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

ik success bug identified.

Browse Source
This commit is contained in:
LiuzhengSJ
2026-07-06 11:10:35 +01:00
parent fb414078f1
commit e06e48f21b
3 changed files with 32 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
kine_ctrl/workspace_comfortable/workspace_cal.py
View File

@ -47,6 +47,27 @@ from Robotic_Arm.rm_robot_interface import *
import time
from math import radians, degrees, pi, cos, sin
# Cartesian workspace grid, in meters.
# Adjust according to your robot placement and task.
X_RANGE = (-0.6, 0.6)
Y_RANGE = (-0.6, 0.6)
Z_RANGE = (0.0, 0.8)
GRID_RESOLUTION = 0.025 # 5 cm. Use 0.02 for finer but slower.
# Comfort thresholds
MIN_JOINT_MARGIN = 0.05 # 15% away from joint limits
MAX_CONDITION_NUMBER = 150.0
MIN_MANIPULABILITY_RATIO = 0.10
# Scoring weights
WEIGHT_IK_SUCCESS = 0.70
WEIGHT_JOINT_LIMIT = 0.10
WEIGHT_MANIPULABILITY = 0.1
WEIGHT_SINGULARITY = 0.1
# 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
@ -98,24 +119,7 @@ JOINT_NAMES = [
"joint_7",
]
# Cartesian workspace grid, in meters.
# Adjust according to your robot placement and task.
X_RANGE = (-0.6, 0.6)
Y_RANGE = (-0.6, 0.6)
Z_RANGE = (0.0, 0.8)
GRID_RESOLUTION = 0.025 # 5 cm. Use 0.02 for finer but slower.
# Comfort thresholds
MIN_JOINT_MARGIN = 0.05 # 15% away from joint limits
MAX_CONDITION_NUMBER = 150.0
MIN_MANIPULABILITY_RATIO = 0.10
# Scoring weights
WEIGHT_IK_SUCCESS = 0.70
WEIGHT_JOINT_LIMIT = 0.10
WEIGHT_MANIPULABILITY = 0.1
WEIGHT_SINGULARITY = 0.1
# Numerical Jacobian settings
JACOBIAN_EPS = 1e-5