add test files for the rm75 kinematics class.

kine_ctrl/test1.py

@@ -0,0 +1,48 @@
+
+
+#   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}')
+
+
+
+
+
+if __name__ == "__main__":
+    main()