diff --git a/kine_ctrl/main.py b/kine_ctrl/main.py
index 2dbe49d..e36dd47 100644
--- a/kine_ctrl/main.py
+++ b/kine_ctrl/main.py
@@ -43,10 +43,13 @@ def main():
# ----------- rm75 qp based kine ------------
- robot_kine_qp = kine_qp(urdf_path='./urdf_rm75/RM75-B.urdf', mesh_dir='./urdf_rm75')
+ robot_kine_qp = kine_qp(urdf_path='./urdf_rm75/RM75-SCI.urdf', mesh_dir='./urdf_rm75')
robot_kine_qp.add_tool_frames(tools_in_ee)
robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
+ fp = robot_kine_qp.forward_kinematics(np.zeros(7))
+ print(f'forward kine res = {fp}')
+
# ---------- rm75 official algorithm -----------
robot_kine_rm = kine_rm()
robot_kine_rm.add_tool_frames(tools_in_ee)
diff --git a/kine_ctrl/rm75_kine_qp.py b/kine_ctrl/rm75_kine_qp.py
index f1a0989..f0293ac 100644
--- a/kine_ctrl/rm75_kine_qp.py
+++ b/kine_ctrl/rm75_kine_qp.py
@@ -71,6 +71,13 @@ class KinematicsSolver():
0.3, 0.3, 0.2
])
+ self.scissor_frame_id = self.model.getFrameId("scissor_tcp")
+ self.camera_frame_id = self.model.getFrameId("camera_tcp")
+
+
+
+
+
def add_frame(self,frame_name, position, rotationXYZ):
'''
:param frame_name: str
@@ -135,7 +142,8 @@ class KinematicsSolver():
pin.updateFramePlacements(self.model, self.data)
# Get frame transform
- frame_id = self.tool_frames[tool]
+ # frame_id = self.tool_frames[tool]
+ frame_id = self.scissor_frame_id
frame_transform = self.data.oMf[frame_id]
# Extract results
diff --git a/kine_ctrl/urdf_rm75/RM75-SCI-V1.urdf b/kine_ctrl/urdf_rm75/RM75-SCI-V1.urdf
new file mode 100644
index 0000000..d003f19
--- /dev/null
+++ b/kine_ctrl/urdf_rm75/RM75-SCI-V1.urdf
@@ -0,0 +1,516 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/kine_ctrl/urdf_rm75/RM75-SCI-V2.urdf b/kine_ctrl/urdf_rm75/RM75-SCI-V2.urdf
new file mode 100644
index 0000000..0ea7320
--- /dev/null
+++ b/kine_ctrl/urdf_rm75/RM75-SCI-V2.urdf
@@ -0,0 +1,516 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/kine_ctrl/urdf_rm75/RM75-SCI.urdf b/kine_ctrl/urdf_rm75/RM75-SCI.urdf
index dfe3b6e..c2881e4 100644
--- a/kine_ctrl/urdf_rm75/RM75-SCI.urdf
+++ b/kine_ctrl/urdf_rm75/RM75-SCI.urdf
@@ -469,7 +469,7 @@
@@ -481,7 +481,7 @@
@@ -491,6 +491,26 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/kine_ctrl/urdf_rm75/meshes/sci.stl b/kine_ctrl/urdf_rm75/meshes/sci.stl
deleted file mode 100644
index 3a4ca52..0000000
Binary files a/kine_ctrl/urdf_rm75/meshes/sci.stl and /dev/null differ