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

6
kine_ctrl/fix_robotics_env.sh Executable file
View File

@ -0,0 +1,6 @@
#!/bin/bash
echo "Fixing robotics environment..."
conda activate coppeliasim
export PYTHONPATH="/home/zl/miniforge3/envs/coppeliasim/lib/python3.10/site-packages"
pip install osqp==0.6.2.post8 --force-reinstall
python -c "import osqp; print(f'OSQP version: {osqp.__version__}')"

8
kine_ctrl/main.py
View File

@ -32,11 +32,11 @@ def main():
if True:
# ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0])
# lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0])
ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0])/180*pi
lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0])/180*pi
ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
lb = -ub
# ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
# lb = -ub
robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)

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]

2
kine_ctrl/rm75_kine_rm.py
View File

@ -16,7 +16,7 @@ class rm75_kine_api():
self.tool_frames = {
'ee': rm_frame_t(frame_name="ee", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
'scissor': rm_frame_t(frame_name="scissor", pose=(0.0, 0.0, 0.144, 0.0, 0, 0.0), payload=1, x=0, y=0, z=72),
'scissor': rm_frame_t(frame_name="scissor", pose=(0.0, 0.0, 0.19, 0.0, 0, 0.0), payload=1, x=0, y=0, z=72),
'camera': rm_frame_t(frame_name="camera", pose=(0.05, 0.02, 0.10, -1.57, 0, -1.57), payload=1, x=0, y=0, z=72)
}
self.work_frames = {