位置控制

Massage/MassageControl/MassageRobot_nova5.py

@@ -274,12 +274,18 @@ class MassageRobot:
                 self.logger.log_blue(f"orientation current: {R.from_quat(quat_rot).as_euler('xyz',degrees=False)}")
                 self.command_rate.precise_sleep()
                 
-            poistion ,quat_rot = self.arm.get_arm_position()
+            position ,quat_rot = self.arm.get_arm_position()
-            self.arm_state.desired_position = poistion
+            # self.arm_state.desired_position = poistion
-            self.arm_state.arm_position_command = poistion
+            self.arm_state.arm_position_command = position
-            self.arm_state.desired_orientation = quat_rot
+            # self.arm_state.desired_orientation = quat_rot
             self.arm_state.arm_orientation_command = quat_rot
             
+            ## POSITION CONTROLLER TEST ##
+            delta_pos = np.array([0.1,0.1,-0.1],dtype=np.float64)
+            # delta_quat = R.from_euler('xyz', [0, 0, 0], degrees=True).as_quat()
+            self.arm_state.desired_position = position + delta_pos
+            self.arm_state.desired_orientation = quat_rot
+            
             # 线程启动
             self.arm_control_thread.start() ## 赋初始值后控制线程
             self.logger.log_info("MassageRobot启动")
@@ -371,9 +377,12 @@ if __name__ == "__main__":
     
     robot.init_hardwares(ready_pose)
     time.sleep(3)
-    robot.controller_manager.switch_controller('admittance')
+    # robot.controller_manager.switch_controller('admittance')
-    atexit.register(robot.force_sensor.disconnect)
+    robot.controller_manager.switch_controller('position')
     
+    
+    
+    atexit.register(robot.force_sensor.disconnect)
     robot.arm_state.desired_wrench = np.array([0,0,-0.5,0,0,0])
     try:
         robot_thread = threading.Thread(target=robot.start)

Massage/MassageControl/config/position.yaml

@@ -1,8 +1,8 @@
 name: position
 
-Kp: [70,70,70,7,7,7]
+Kp: [20,20,20,1,1,1]
-Ki: [0, 0, 0, 0, 0, 0]
+Ki: [0.2,0.2,0.2, 0, 0, 0]
-Kd: [5, 5, 5, 1, 1, 1]
+Kd: [10,10,10, 1, 1, 1]
 
 # Kp: [60,60,60,6,6,6]
 # Ki: [10, 10, 10, 1, 1, 1]

Massage/MassageControl/hardware/dobot_nova5.py

@@ -732,6 +732,11 @@ class dobot_nova5:
         
 
     def get_arm_position(self):
+        '''
+        输出pos,quat
+        pos: [x,y,z] in m
+        quat: [qx,qy,qz,qw] 四元数
+        '''
         pose = self.Get_feedInfo_pose()
         # print("pose",pose)
         x,y,z,rx,ry,rz = pose # x y z in mm; rx ry rz in degree
@@ -814,8 +819,7 @@ if __name__ == "__main__":
     dobot.chooseEndEffector(i=9)
     print("Arm pose:",dobot.getPose())
 
-    # dobot.RunPoint_P_inPose([, 0, 154.071, 0, 0, 0])
+    # dobot.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90])
 
-    
     # dobot.start_drag()
     dobot.disableRobot()