优化远控

robot_client/robots/realman/motors_bus.py

@@ -144,9 +144,11 @@ class RealmanMotorsBus(MotorsBus):
             time.sleep(1)
         
         self.rmarm.rm_movej(self.init_joint_position[:-1], 5, 0, 0, 1)
-        time.sleep(3)
-        ret = self.rmarm.rm_get_current_arm_state()
         self.init_pose = ret[1]['pose']
+        #读取夹状态
+        gripper_msg = self.rmarm.rm_get_gripper_state()[1]
+        gripper_state = gripper_msg['actpos']
+        self.init_pose.append(gripper_state)
         resp = enable_flag
         print(f"Returning response: {resp}")
         return resp
@@ -201,12 +203,6 @@ class RealmanMotorsBus(MotorsBus):
             "gripper": (gripper_state-500)/500
         }
     
-    def read_current_arm_joint_state(self):
-        return self.rmarm.rm_get_current_arm_state()[1]['joint']
-    
-    def read_current_arm_endpose_state(self):
-        return self.rmarm.rm_get_current_arm_state()[1]['pose']
-    
     def disconnect(self, disable_torque = True):
         self.safe_disconnect()
 
@@ -225,12 +221,14 @@ class RealmanMotorsBus(MotorsBus):
         if name =="Goal_Position":
             self.write_arm(target_joint=actionData)
         elif name == "Goal_Velocity":#速度透传模式
-            self.rmarm.rm_movev_canfd(list(actionData.values())[:6])
-            gripper = actionData['gripper']
-            if gripper > 0 :
-                self.rmarm.rm_set_gripper_release(gripper*1000, False)
-            else:
-                self.rmarm.rm_set_gripper_pick_on(gripper*1000, gripper*1000, False)
+            values = list(actionData.values())
+            for k,v in enumerate(self.init_pose):
+                self.init_pose[k]+=values[k]
+            values[-1]
+            self.rmarm.rm_movev_canfd(self.init_pose[:6])
+            #把夹爪的输入范围-1到1映射为0到1
+            gripper = (actionData['gripper']+1)/2
+            self.rmarm.rm_set_gripper_position(gripper*1000, False)
     
     def sync_read(self, data_name, motors = None, *, normalize = True, num_retry = 0):
         if data_name == "Present_Position":