update arm ctrl rm py

harvest_arm_ctrl/config/config.yaml

@@ -0,0 +1,37 @@
+
+harvest_arm_ctrl:
+  ros__parameters:
+
+    urdf_path:  '/home/zl/ws_Harvest/src/harvest_arm_rm/urdf/urdf_rm75/RM75.urdf'
+    meshes_path: '/home/zl/ws_Harvest/src/harvest_arm_rm/urdf/urdf_rm75'
+    
+    qp_iteration: 200
+    joint_upper : [155.0, 115.0, 172.0,  130, 175.0, 125.0,  350.0 ]
+    joint_lower : [-155.0, -30.0, -172.0, -130, -175.0, -125.0, -350.0 ]
+
+    tool_name: "scissor"
+
+    #   0~2 position, x, y, z, m
+#   3~6 orientation, quaternion, x, y, z, w
+#   7 mass, kg
+#   8~10 center of mass, x, y, z, m
+    tools_in_ee.scissor: [
+      0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0,
+      0.66, 0.0, 0.0, 0.06
+    ]
+
+    tools_in_ee.omnipic: [
+      0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0,
+      0.43, 0.0, 0.0, 0.06
+    ]
+
+    tools_in_ee.minisci: [
+      0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0,
+      0.46, 0.0, 0.0, 0.06
+    ]
+
+    tools_in_ee.no_tool: [
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
+      0.0, 0.0, 0.0, 0.0
+    ]
+

harvest_arm_ctrl/harvest_arm_ctrl/arm_ctrl.py

@@ -1,6 +0,0 @@
-def main():
-    print('Hi from harvest_arm_ctrl.')
-
-
-if __name__ == '__main__':
-    main()

harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_qp.py

@@ -12,7 +12,7 @@ import threading
 
 
 
-class KinematicsSolver():
+class KinematicsSolver_QP():
     def __init__(self,urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir="urdf_rm75"):
         """
         for realman 75b
@@ -712,7 +712,7 @@ def main():
     """Main test function"""
 
 
-    rm75 = KinematicsSolver()
+    rm75 = KinematicsSolver_QP()
 
     # Test 1: Forward Kinematics
     print("\n1. Forward Kinematics Test")

harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_rm.py

@@ -7,7 +7,7 @@ class rm75_kine_api():
     def __init__(self):
         #   ----------      rm75 official algorithm         -----------
         print(f'-------  the realman official kinematic initialising -------')
-        arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E  # RM_65 Robotic arm
+        arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E  # RM_75 Robotic arm
         force_type = rm_force_type_e.RM_MODEL_RM_B_E  # Standard version
         # Initialize the robotic arm model and sensor type in the algorithm
         self.robot_kine_rm = Algo(arm_model, force_type)
@@ -120,14 +120,37 @@ class rm75_kine_api():
             self.work_name = work
             self.cfg_work_frame(work)
 
-        target = target_position + target_rpy
+        target = list(target_position) + list(target_rpy)
 
         if initial_guess is not None:
             q_ref = [ 180/math.pi * ig for ig in  initial_guess ]
         else:
             q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0]
         ret, phi = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref)
+        # print(f'the arm angle is ret = {ret}, and phi = {phi}')
         params = rm_inverse_kinematics_params_t(q_ref,
                                                 target, 1)
         ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi)
-        return ret, [ q/180*math.pi for q in q_out]
+        p_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1)
+        pose_dis = cal_pose_deviation(p_fk, target)
+
+        # print(f'target pose is {target}, fk pose is {p_fk}, dis of poses is {pose_dis}')
+        #
+        # print(f'\nin the rm75_kine_rm, l133, inverse_kinematics, q_ref = {q_ref}, target = {target}  phi = {phi}, q_out = {q_out}, ret = {ret}\n\n')
+        # print(f'the tool frame is {self.robot_kine_rm.rm_algo_get_curr_toolframe()}')
+        if int(ret) < 0:
+            return ret, [ q/180*math.pi for q in q_out]
+        elif pose_dis < 0.01:
+            return ret, [ q/180*math.pi for q in q_out]
+        else:
+            return -10, [ q/180*math.pi for q in q_out]
+
+def cal_pose_deviation(pose1, pose2):
+    d_fk_p1 = np.array(pose1) - np.array(pose2)
+    for j in [3, 4, 5]:
+        while d_fk_p1[j] > math.pi:
+            d_fk_p1[j] -= 2 * math.pi
+        while d_fk_p1[j] < -math.pi:
+            d_fk_p1[j] += 2 * math.pi
+    d_fk = np.linalg.norm(d_fk_p1)
+    return d_fk

harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics.py

@@ -1,6 +0,0 @@
-def main():
-    print('Hi from harvest_arm_ctrl.')
-
-
-if __name__ == '__main__':
-    main()

harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics_arm.py

@@ -0,0 +1,11 @@
+from harvest_arm_ctrl.rm75_kine_qp import KinematicsSolver_QP as kine_qp
+from harvest_arm_ctrl.rm75_kine_rm import KinematicsSolver_RM as kine_rm
+
+
+
+def main():
+    print('Hi from harvest_arm_ctrl.')
+
+
+if __name__ == '__main__':
+    main()

harvest_arm_ctrl/setup.py

@@ -24,7 +24,7 @@ setup(
     },
     entry_points={
         'console_scripts': [
-            'arm_ctrl = harvest_arm_ctrl.arm_ctrl:main'
+            'kinematics_arm = harvest_arm_ctrl.run_kinematics_arm:main',
         ],
     },
 )