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

17
kine_ctrl/main.py
View File

@ -48,14 +48,15 @@ def main():
robot_kine_rm.add_tool_frames(tools_in_ee) robot_kine_rm.add_tool_frames(tools_in_ee)
robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True) robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
# ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=[-0.6, -0.6 , 0. ], target_rpy=[1.2022060487764064, -1.0097962261845583, -0.6518417572686532], ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=[-0.6, -0.6 , 0. ], target_rpy=[1.2022060487764064, -1.0097962261845583, -0.6518417572686532],
# initial_guess=[0.1] * 7, tool="no_tool") initial_guess=[0.1] * 7, tool="no_tool")
#
# print(f'ret_rm = {ret_rm}, q = {q}') print(f'ret_rm = {ret_rm}, q = {q}')
# pose = robot_kine_rm.forward_kinematics(joint_angles=q, tool="no_tool") pose = robot_kine_rm.forward_kinematics(joint_angles=q, tool="no_tool")
# print(f'pose = {pose}') print(f'pose = {pose}')
# print('-'*100)
# time.sleep(5)
time.sleep(5)

3
kine_ctrl/rm75_kine_rm.py
View File

@ -137,7 +137,8 @@ class rm75_kine_api():
# print(f'target pose is {target}, fk pose is {pose_fk}, dis of poses is {pose_dis}') # print(f'target pose is {target}, fk pose is {pose_fk}, dis of poses is {pose_dis}')
# #
# print(f'\nin the rm75_kine_rm, l133, inverse_kinematics, q_ref = {q_ref}, target = {target} phi = {phi}, q_out = {q_out}, ret = {ret}\n\n') # print(f'\nin the rm75_kine_rm, l133, inverse_kinematics, q_ref = {q_ref}, target = {target} phi = {phi}, q_out = {q_out}, ret = {ret}\n\n')
if ret < 0: # print(f'the tool frame is {self.robot_kine_rm.rm_algo_get_curr_toolframe()}')
if int(ret) < 0:
return ret, [ q/180*math.pi for q in q_out] return ret, [ q/180*math.pi for q in q_out]
elif pose_dis < 0.01: elif pose_dis < 0.01:
return ret, [ q/180*math.pi for q in q_out] return ret, [ q/180*math.pi for q in q_out]

38
kine_ctrl/workspace_comfortable/workspace_cal.py
View File

@ -47,6 +47,27 @@ from Robotic_Arm.rm_robot_interface import *
import time import time
from math import radians, degrees, pi, cos, sin 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 # 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 # 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", "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 # Numerical Jacobian settings
JACOBIAN_EPS = 1e-5 JACOBIAN_EPS = 1e-5