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_rpy = [0.78576018, 0.67554633, 1.86302226]
target_p = target_position + target_rpy target_p = target_position + target_rpy
# target_p_rad = [radians(pos) for pos in 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 ] initial_guess_rad = [ radians(j) for j in initial_guess ]
tool_name = "scissor" tool_name = "scissor"
@ -71,12 +71,15 @@ def demo_position_control():
joint_solution, success, error = robot_kine_qp.inverse_kinematics( joint_solution, success, error = robot_kine_qp.inverse_kinematics(
target_p[0:3], target_rpy=target_p[3:6], initial_guess=initial_guess_rad, 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: 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') 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')

21
kine_ctrl/rm75_kine_qp.py
View File

@ -255,7 +255,7 @@ class KinematicsSolver():
error_SE3 = current_placement.actInv(target_placement) error_SE3 = current_placement.actInv(target_placement)
error_vec = pin.log(error_SE3).vector error_vec = pin.log(error_SE3).vector
print("initial error =", np.linalg.norm(error_vec)) print("\n initial error =", np.linalg.norm(error_vec))
print(error_vec) print(error_vec)
while iter_count < max_iter: while iter_count < max_iter:
@ -295,7 +295,7 @@ class KinematicsSolver():
# ========================= # =========================
# QP-based IK # QP-based IK
# ========================= # =========================
w_posture = 0.0 w_posture = 0.0001
J_eff = pin.Jlog6(error_SE3) @ J #J # J_eff = pin.Jlog6(error_SE3) @ J #J #
@ -348,6 +348,14 @@ class KinematicsSolver():
print("error", error_norm) print("error", error_norm)
print("cond(H)", np.linalg.cond(H)) print("cond(H)", np.linalg.cond(H))
u, s, vh = np.linalg.svd(J_eff)
print("sv =", s)
print("trans =", error_vec[:3])
print("rot =", error_vec[3:])
# Solve # Solve
result = self.osqp_solver.solve() result = self.osqp_solver.solve()
print("OSQP status =", result.info.status) print("OSQP status =", result.info.status)
@ -385,11 +393,10 @@ class KinematicsSolver():
fk0 = self.forward_kinematics(initial_guess) fk0 = self.forward_kinematics(initial_guess)
print("fk guess:", fk0) print("fk guess:", fk0)
print("initial error norm:", error_norm) print(result.info.status)
print(np.degrees(q))
print(iter_count, print(np.degrees(self.model.upperPositionLimit[:7]))
error_norm, print(np.degrees(self.model.lowerPositionLimit[:7]))
result.info.status)
if best_solution is not None: if best_solution is not None:
print( print(