add test files for the rm75 kinematics class.

2026-08-05 14:10:06 +01:00
parent d86438115f
commit 2e25951bc0
3 changed files with 90 additions and 1 deletions
@@ -0,0 +1,90 @@
+
+
+#   conda activate coppeliasim
+#   env fix, in terminal:  fix_robotics_env.sh
+
+from rm75_kinematics import rm75_kinematics
+
+from math import pi
+import numpy as np
+
+#   pose expression of tool-tip in end-effector, x y z quatx quaty quatz quatw
+#   load: kg, mass_center_x in ee frame: m, y, z, then last threes are for filling
+tools_in_ee = {
+    'scissor': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
+    'omnipic': np.array([[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],[0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
+    'minisci': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
+    'no_tool': np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],dtype=np.float64),
+}
+
+# joint limit
+ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
+lb = -ub
+
+tool_name = "scissor"
+
+def main():
+    """Demonstrate pure position control"""
+
+    # Create controller
+
+    robot_kine = rm75_kinematics(urdf_path='./urdf_rm75/RM75-SCI.urdf',mesh_dir='./urdf_rm75',
+                                 tcps=["scissor_tcp", "camera_tcp"],tools_in_ee=tools_in_ee,min_j=lb, max_j=ub)
+
+    ret_ik, q = robot_kine.get_ik_result(target_position=[0.2, -0.2 , 0.5 ], target_rpy=[0.2022060487764064, -0.0097962261845583, -0.6518417572686532],
+                                                 initial_guess=[0.1] * 7, tool=tool_name)
+
+    self_collision_sts = robot_kine.get_self_collision(q)
+
+    p = robot_kine.get_fk_result(joint_angles=q,tool=tool_name)
+
+    print(f'self_collision_sts: {self_collision_sts}')
+
+    import numpy as np
+
+    ik_suc = 0
+
+    for i in range(100):
+        joint_rand = np.random.uniform(ub, lb)
+
+        p_t = robot_kine.get_fk_result(joint_angles=joint_rand.tolist(), tool=tool_name)
+
+        joint_rand_init = np.random.uniform(ub, lb)
+
+        ret_ik, q = robot_kine.get_ik_result(target_position=p_t[0:3], target_rpy=p_t[3:6],
+                                             initial_guess=joint_rand_init, tool=tool_name)
+
+
+
+
+        p_fk = robot_kine.get_fk_result(joint_angles=q, tool=tool_name)
+
+        d_p_ik = cal_pose_deviation(pose1=p_t, pose2=p_fk)
+
+        coll_sts = robot_kine.get_self_collision(joint_angles=q)
+        if ret_ik == True:
+
+            print(f'\n---- success, in the ik, j_t = {joint_rand}, q = {q}, p_t = {p_t}, d_p_ik = {d_p_ik}, self-collision_sts = {coll_sts}')
+
+            ik_suc += 1
+        else:
+            print(f'\n**** ik failed, in the ik, j_t = {joint_rand}, q={q}, p_t = {p_t}, d_p_ik = {d_p_ik}, self-collision_sts = {coll_sts}')
+
+
+    print(f'ik_suc: {ik_suc}')
+
+
+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] > pi:
+            d_fk_p1[j] -= 2 * pi
+        while d_fk_p1[j] < -pi:
+            d_fk_p1[j] += 2 * pi
+    d_fk = np.linalg.norm(d_fk_p1)
+    return d_fk
+
+
+
+if __name__ == "__main__":
+    main()