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

correct the w_posture

Browse Source
This commit is contained in:
LiuzhengSJ
2026-06-04 14:31:29 +01:00
parent c0458f0e12
commit f10152fc29
2 changed files with 21 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
kine_ctrl/main.py
View File

@ -61,7 +61,7 @@ def demo_position_control():
target_rpy = [0.78576018, 0.67554633, 1.86302226]
target_p = target_position + target_rpy
# target_p_rad = [radians(pos) for pos in target_position] + target_rpy
initial_guess = [11, 20, -30, -40, 50, 60, 71] # [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0] #
initial_guess = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0] # [0.0, 20, -30, -40, 50, 60, 91] #
initial_guess_rad = [ radians(j) for j in initial_guess ]
tool_name = "scissor"
@ -71,12 +71,15 @@ def demo_position_control():
joint_solution, success, error = robot_kine_qp.inverse_kinematics(
target_p[0:3], target_rpy=target_p[3:6], initial_guess=initial_guess_rad,
max_iter=500, debug=False, tool=tool_name
)
max_iter=500, debug=False, tool=tool_name )
print(f'the qp based kinematics result: {joint_solution}, success: {success}, error: {error}\n')
if success:
print(f'the qp based kinematics result: {joint_solution}, success: {success}, error: {error}\n')
print(f'forward result of the ik solution is {robot_kine_qp.forward_kinematics(joint_solution , tool=tool_name)}\n')
else:
print(f'solution: {joint_solution} success flag {success}, error {error}\n')
print(
f'forward result of the ik solution is {robot_kine_qp.forward_kinematics(joint_solution, tool=tool_name)}\n')