robot arm → robot pc에서 StartStateUpdate 함수로 waist, right arm, left arm 등의 joint state 를 특정 주기(Hz) 마다 얻어 올 수 있다.

그러나 gripper joint의 경우 따로 받아 오는 함수가 없어서 직접 만들어줌

master arm 과의 joint mapping을 통해 로봇을 control 하는 teleoperation_with_joint_mapping.cpp 코드에서 master arm handler 와 gripper handler 총 2가지 thread가 있는데, gripper position을 받아오는 함수(GetGripperPosition)를 gripper handler에 추가해주었다.

master arm handler : master arm의 button을 누르면 joint space controller 를 통해 joint mapping

gripper handler : gripper의 q값의 min, max값을 측정하고 이를 바탕으로 operational mode에 돌입해 master arm의 trigger의 위로 올림 내림을 gripper 열리고 닫힘으로 mapping 한다.

우선 로봇은 gripper dynamixel의 encoder를 통해 그리퍼가 열렸을 때, q의 min, max값을 initialize 하는데, q의 min 값이 그리퍼가 열렸을 때로 대략 -440 정도, q의 max값이 그리퍼가 닫혔을 때로 대략 90정도로 측정된다.

따라서 현재 q의 value값을 받아오고 이를 0~1사이로 normalize 함으로써 gripper position을 받아올 수 있다.

• q_min_max_vector[id](0 or 1) 에서 id는 left(1)냐 right(0)냐 이고 0,1은 각각 열림과 닫힘이다.
• SendCurrent(portHandler, packetHandler, id, 0.5) → gripper를 닫는 명령
• SendCurrent(portHandler, packetHandler, id, -0.5) → gripper를 여는 명령

TODO : robot pc에서 joint state를 받아오는 주기와 master arm으로 부터 gripper state를 받아오는 주기를 일치 시켜야 함.