ZhengLiu-cart/IK_qp
2
1
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

start to adjust to ready-to-use class

Browse Source
This commit is contained in:
LiuzhengSJ
2026-06-22 13:29:58 +01:00
parent 2ca5033b46
commit 6c8a335e1d
4 changed files with 28 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

63
kine_ctrl/rm75_kine_qp.py
View File

@ -8,6 +8,7 @@ import osqp
from scipy import sparse
from math import radians, degrees, pi, cos, sin
import time
import threading
@ -24,61 +25,18 @@ class KinematicsSolver():
# -------------------------------------------------
# ee
# -------------------------------------------------
ee_offset = pin.SE3(np.eye(3), np.array([0, 0, 0.0]))
self.model.addFrame(
pin.Frame(
"ee",
self.model.getJointId("joint_7"),
self.model.getFrameId("link_7"),
ee_offset,
pin.FrameType.OP_FRAME
)
)
self.add_frame(frame_name="ee", position=[0.0, 0.0, 0.0], rotationXYZ=[0.0, 0.0, 0.0])
# -------------------------------------------------
# Scissor tool
# -------------------------------------------------
scissor_offset = pin.SE3(
np.eye(3),
np.array([0.0, 0.0, 0.144])
)
self.model.addFrame(
pin.Frame(
"scissor",
self.model.getJointId("joint_7"),
self.model.getFrameId("link_7"),
scissor_offset,
pin.FrameType.OP_FRAME
)
)
self.add_frame(frame_name="scissor", position=[0.0, 0.0, 0.19], rotationXYZ=[0.0, 0.0, 0.0])
# -------------------------------------------------
# Camera tool
# -------------------------------------------------
camera_rotation = pin.rpy.rpyToMatrix(
radians(-90),
0,
radians(-90)
)
camera_offset = pin.SE3(
camera_rotation,
np.array([0.05, 0.02, 0.10])
)
self.model.addFrame(
pin.Frame(
"camera",
self.model.getJointId("joint_7"),
self.model.getFrameId("link_7"),
camera_offset,
pin.FrameType.OP_FRAME
)
)
self.add_frame(frame_name="camera",position=[0.05, 0.02, 0.10], rotationXYZ=[-pi*0.5, 0.0, -pi*0.5])
# -------------------------------------------------
# Store tool frame IDs
# -------------------------------------------------
self.tool_frames = {
"scissor": self.model.getFrameId("scissor"),
"camera": self.model.getFrameId("camera"),
@ -118,6 +76,19 @@ class KinematicsSolver():
self.W = np.diag([1, 1, 1, 0.4, 0.4, 0.4])
def add_frame(self,frame_name, position, rotationXYZ):
'''
:param frame_name: str
:param position: [x, y, z] target position (meters)
:param rotationXYZ: [x, y, z] target rotation (rad)
'''
camera_rotation = pin.rpy.rpyToMatrix( rotationXYZ[0], rotationXYZ[1], rotationXYZ[2] )
camera_offset = pin.SE3(
camera_rotation,
np.array(position)
)
self.model.addFrame( pin.Frame( frame_name, self.model.getJointId("joint_7"), self.model.getFrameId("link_7"), camera_offset, pin.FrameType.OP_FRAME ) )
def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True):
if min_j is None:
min_j = [-3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -6.14159]