ZhongyuWang-cart/IK_qp
1
0
Fork 0
forked from ZhengLiu-cart/IK_qp
Code Pull Requests Activity

1. add q_mid and mid weight, making the joint prefer to stay at the middle of joint range.

Browse Source 
2. add dq_limit weight, making the last several joints move more proactively.
This commit is contained in:
LiuzhengSJ
2026-07-01 15:42:21 +01:00
parent 4b5eeccf7f
commit 7246710a7d
1 changed files with 23 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
kine_ctrl/rm75_kine_qp.py
View File

@ -26,6 +26,15 @@ class KinematicsSolver():
self.cfg_j_limit()
q_range = (
self.model.upperPositionLimit[:7] -
self.model.lowerPositionLimit[:7]
)
self.w_q_limit = np.diag(1.0 / (q_range ** 2))
self.q_mid = 0.5 * (self.model.lowerPositionLimit[:7] + self.model.upperPositionLimit[:7])
# ---------- for reused qp_solver ------------------
self.nv = 7
@ -51,6 +60,12 @@ class KinematicsSolver():
)
self.W = np.diag([1, 1, 1, 0.4, 0.4, 0.4])
# Smaller value => joint moves more actively
# Larger value => joint moves less / more lazy
self.joint_motion_weight = np.diag([
1.0, 1.0, 1.0, 1.0,
0.3, 0.3, 0.2
])
def add_frame(self,frame_name, position, rotationXYZ):
'''
@ -255,28 +270,28 @@ class KinematicsSolver():
# =========================
# QP-based IK
# =========================
w_posture = 0.0001
w_ref = 0.0001
w_limit_mid = 0.00002
J_eff = pin.Jlog6(error_SE3) @ J #J #
H = J_eff.T @ self.W @ J_eff
# H = J.T @ self.W @ J
H += damping * damping * np.eye(7)
H += w_posture * np.eye(7)
H += damping * damping * self.joint_motion_weight
H += w_ref * np.eye(7)
H += w_limit_mid * self.w_q_limit
H_triu = sparse.triu(H).tocsc()
g = -J_eff.T @ self.W @ error_vec
g += w_posture * (q[:7] - q_ref[:7])
# g = - J.T @ self.W @ error_vec
g += w_ref * (q[:7] - q_ref[:7])
g += w_limit_mid * self.w_q_limit @ (q[:7] - self.q_mid)
# -------------------------
# Joint velocity constraints
# -------------------------
dq_limit = 0.05 # rad per iteration
dq_limit = np.array([ 0.05, 0.05, 0.05, 0.05, 0.08, 0.08, 0.10 ]) # rad per iteration
lb = -dq_limit * np.ones(7)
ub = dq_limit * np.ones(7)