forked from ZhengLiu-cart/IK_qp
refine qp based controller
This commit is contained in:
LiuzhengSJ
@ -16,8 +16,9 @@ class KinematicsSolver():
|
||||
"""
|
||||
for realman 75b
|
||||
Initialize robotic arm kinematics using Pinocchio (ROS2 version).
|
||||
unit: m, rad
|
||||
"""
|
||||
|
||||
print(f' ------------ the qp based kinematic initialising -----------')
|
||||
self.model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
|
||||
|
||||
# -------------------------------------------------
|
||||
@ -44,7 +45,7 @@ class KinematicsSolver():
|
||||
)
|
||||
self.model.addFrame(
|
||||
pin.Frame(
|
||||
"scissor_tcp",
|
||||
"scissor",
|
||||
self.model.getJointId("joint_7"),
|
||||
self.model.getFrameId("link_7"),
|
||||
scissor_offset,
|
||||
@ -66,7 +67,7 @@ class KinematicsSolver():
|
||||
)
|
||||
self.model.addFrame(
|
||||
pin.Frame(
|
||||
"camera_frame",
|
||||
"camera",
|
||||
self.model.getJointId("joint_7"),
|
||||
self.model.getFrameId("link_7"),
|
||||
camera_offset,
|
||||
@ -79,8 +80,8 @@ class KinematicsSolver():
|
||||
# -------------------------------------------------
|
||||
|
||||
self.tool_frames = {
|
||||
"scissor": self.model.getFrameId("scissor_tcp"),
|
||||
"camera": self.model.getFrameId("camera_frame"),
|
||||
"scissor": self.model.getFrameId("scissor"),
|
||||
"camera": self.model.getFrameId("camera"),
|
||||
"ee": self.model.getFrameId("ee")
|
||||
}
|
||||
|
||||
@ -106,9 +107,10 @@ class KinematicsSolver():
|
||||
self.nv = 7
|
||||
|
||||
# Full dense symmetric matrix structure
|
||||
P_template = np.triu(np.ones((7, 7)))
|
||||
# P_template = np.triu(np.ones((7, 7)))
|
||||
self.P_pattern = sparse.triu(np.ones((7,7))).tocsc()
|
||||
|
||||
P_sparse = sparse.csc_matrix(P_template)
|
||||
P_sparse = sparse.csc_matrix(self.P_pattern)
|
||||
|
||||
A_sparse = sparse.eye(7, format='csc')
|
||||
|
||||
@ -138,6 +140,8 @@ class KinematicsSolver():
|
||||
|
||||
Returns:
|
||||
dict: Position, rotation, rpy, quaternion
|
||||
unit: position: m
|
||||
rpy: rad
|
||||
"""
|
||||
if len(joint_angles) != 7:
|
||||
raise ValueError(f"RM75 has 7 joints, got {len(joint_angles)}")
|
||||
@ -167,15 +171,15 @@ class KinematicsSolver():
|
||||
|
||||
return {
|
||||
'position': position,
|
||||
'rotation': rotation,
|
||||
# 'rotation': rotation,
|
||||
'rpy': rpy,
|
||||
'quaternion': [quat.x, quat.y, quat.z, quat.w],
|
||||
'transform': frame_transform
|
||||
# 'transform': frame_transform
|
||||
}
|
||||
|
||||
def inverse_kinematics(self, target_position, target_rpy=None,
|
||||
target_quat=None, initial_guess=None,
|
||||
max_iter=200, tolerance=1e-3, debug=False, tool="ee"):
|
||||
max_iter=500, tolerance=3e-3, debug=False, tool="ee"):
|
||||
"""
|
||||
Compute inverse kinematics using differential IK with multiple strategies.
|
||||
|
||||
@ -230,7 +234,7 @@ class KinematicsSolver():
|
||||
self.model.upperPositionLimit[i])
|
||||
|
||||
# Differential IK with adaptive damping
|
||||
damping = 0.01
|
||||
damping = 0.1
|
||||
damping_reduction = 0.95
|
||||
iter_count = 0
|
||||
prev_error = float('inf')
|
||||
@ -242,9 +246,20 @@ class KinematicsSolver():
|
||||
self.data,
|
||||
q,
|
||||
ee_frame_id,
|
||||
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
|
||||
pin.ReferenceFrame.LOCAL
|
||||
)
|
||||
|
||||
pin.forwardKinematics(self.model, self.data, q)
|
||||
pin.updateFramePlacements(self.model, self.data)
|
||||
|
||||
current_placement = self.data.oMf[ee_frame_id]
|
||||
|
||||
error_SE3 = current_placement.actInv(target_placement)
|
||||
error_vec = pin.log(error_SE3).vector
|
||||
|
||||
print("initial error =", np.linalg.norm(error_vec))
|
||||
print(error_vec)
|
||||
|
||||
while iter_count < max_iter:
|
||||
# Compute forward kinematics
|
||||
|
||||
@ -281,7 +296,7 @@ class KinematicsSolver():
|
||||
self.model,
|
||||
self.data,
|
||||
ee_frame_id,
|
||||
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
|
||||
pin.ReferenceFrame.LOCAL
|
||||
)
|
||||
|
||||
# =========================
|
||||
@ -293,7 +308,7 @@ class KinematicsSolver():
|
||||
|
||||
H_triu = sparse.triu(H).tocsc()
|
||||
|
||||
g = -J.T @ self.W @ error_vec
|
||||
g = - J.T @ self.W @ error_vec
|
||||
|
||||
# -------------------------
|
||||
# Joint velocity constraints
|
||||
@ -319,20 +334,32 @@ class KinematicsSolver():
|
||||
# ------------------------
|
||||
# Update solver
|
||||
self.osqp_solver.update(
|
||||
Px=H_triu.data,
|
||||
Px= H_triu.data, #H[np.triu_indices(7)], #
|
||||
q=g,
|
||||
l=lb,
|
||||
u=ub
|
||||
)
|
||||
|
||||
|
||||
|
||||
print("iter", iter_count)
|
||||
print("error", error_norm)
|
||||
print("cond(H)", np.linalg.cond(H))
|
||||
|
||||
# Solve
|
||||
result = self.osqp_solver.solve()
|
||||
print("OSQP status =", result.info.status)
|
||||
print("dq =", result.x)
|
||||
|
||||
if result.x is not None:
|
||||
print("dq norm:", np.linalg.norm(result.x))
|
||||
|
||||
if result.info.status != 'solved':
|
||||
break
|
||||
|
||||
dq = result.x
|
||||
|
||||
print(f'pred = {J @ dq} and error_vec = {error_vec}')
|
||||
|
||||
if dq is None:
|
||||
break
|
||||
@ -344,181 +371,199 @@ class KinematicsSolver():
|
||||
prev_error = error_norm
|
||||
iter_count += 1
|
||||
|
||||
print("target:", target_position, target_rpy)
|
||||
|
||||
print("initial guess:", np.degrees(initial_guess))
|
||||
|
||||
fk0 = self.forward_kinematics(initial_guess)
|
||||
print("fk guess:", fk0)
|
||||
|
||||
print("initial error norm:", error_norm)
|
||||
|
||||
print(iter_count,
|
||||
error_norm,
|
||||
result.info.status)
|
||||
|
||||
if best_solution is not None:
|
||||
return best_solution, True, best_error
|
||||
else:
|
||||
return None, False, None
|
||||
|
||||
def invese_kinematics_velocity(self, target_position, target_rpy=None,
|
||||
target_quat=None, initial_guess=None, tool="ee"):
|
||||
"""
|
||||
Compute the converging velocity (motion direction) of joints based on qp inverse kinematics.
|
||||
|
||||
Args:
|
||||
target_position: [x, y, z] target position (meters)
|
||||
target_rpy: [roll, pitch, yaw] target orientation (radians)
|
||||
target_quat: [x, y, z, w] target orientation as quaternion
|
||||
initial_guess: Initial joint angles (radians)
|
||||
tool: the frame name ('scissor', 'camera', 'ee')
|
||||
|
||||
Returns:
|
||||
joint_velocity: np.array()
|
||||
"""
|
||||
# Build target SE3 placement
|
||||
if target_quat is not None:
|
||||
quat = pin.Quaternion(target_quat[3], target_quat[0],
|
||||
target_quat[1], target_quat[2])
|
||||
target_rotation = quat.matrix()
|
||||
elif target_rpy is not None:
|
||||
target_rotation = pin.rpy.rpyToMatrix(target_rpy[0],
|
||||
target_rpy[1],
|
||||
target_rpy[2])
|
||||
else:
|
||||
target_rotation = np.eye(3)
|
||||
|
||||
target_placement = pin.SE3(target_rotation, np.array(target_position))
|
||||
|
||||
# Try multiple initial guesses
|
||||
initial_guesses = []
|
||||
|
||||
if initial_guess is not None:
|
||||
initial_guesses.append(initial_guess)
|
||||
else:
|
||||
# Try different initial configurations
|
||||
initial_guesses.append([0.1] * 7) # Zero config
|
||||
initial_guesses.append([radians(30), radians(45), radians(30),
|
||||
radians(-45), radians(30), radians(-30), 0])
|
||||
initial_guesses.append([radians(-30), radians(45), radians(-30),
|
||||
radians(45), radians(30), radians(30), 0])
|
||||
|
||||
best_solution = None
|
||||
best_error = float('inf')
|
||||
|
||||
for guess_idx, guess in enumerate(initial_guesses):
|
||||
q = pin.neutral(self.model)
|
||||
for i, angle in enumerate(guess):
|
||||
if i < len(q):
|
||||
q[i] = np.clip(angle, self.model.lowerPositionLimit[i],
|
||||
self.model.upperPositionLimit[i])
|
||||
|
||||
# Differential IK with adaptive damping
|
||||
damping = 0.01
|
||||
damping_reduction = 0.95
|
||||
iter_count = 0
|
||||
prev_error = float('inf')
|
||||
|
||||
ee_frame_id = self.tool_frames[tool]
|
||||
|
||||
J = pin.computeFrameJacobian(
|
||||
self.model,
|
||||
self.data,
|
||||
q,
|
||||
ee_frame_id,
|
||||
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
|
||||
print(
|
||||
"converged",
|
||||
error_norm,
|
||||
position_error
|
||||
)
|
||||
|
||||
while iter_count < max_iter:
|
||||
# Compute forward kinematics
|
||||
|
||||
pin.computeJointJacobians(self.model, self.data, q)
|
||||
pin.framesForwardKinematics(self.model, self.data, q)
|
||||
|
||||
# Get current end-effector placement
|
||||
|
||||
current_placement = self.data.oMf[ee_frame_id]
|
||||
|
||||
# Compute error
|
||||
error_SE3 = current_placement.actInv(target_placement)
|
||||
error_vec = pin.log(error_SE3).vector
|
||||
error_norm = np.linalg.norm(error_vec)
|
||||
|
||||
if error_norm < tolerance:
|
||||
joint_angles = q[:7].copy()
|
||||
fk_result = self.forward_kinematics(joint_angles, tool=tool)
|
||||
position_error = np.linalg.norm(fk_result['position'] - np.array(target_position))
|
||||
|
||||
if position_error < best_error:
|
||||
best_error = position_error
|
||||
best_solution = joint_angles
|
||||
break
|
||||
|
||||
# Check if error is increasing (diverging)
|
||||
if error_norm > prev_error * 1.1 and iter_count > 10:
|
||||
damping = min(1.0, damping * 1.5)
|
||||
else:
|
||||
damping = max(0.01, damping * damping_reduction)
|
||||
|
||||
J = pin.getFrameJacobian(
|
||||
self.model,
|
||||
self.data,
|
||||
ee_frame_id,
|
||||
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
|
||||
)
|
||||
|
||||
# =========================
|
||||
# QP-based IK
|
||||
# =========================
|
||||
|
||||
H = J.T @ self.W @ J
|
||||
H += damping * damping * np.eye(7)
|
||||
|
||||
H_triu = sparse.triu(H).tocsc()
|
||||
|
||||
g = -J.T @ self.W @ error_vec
|
||||
|
||||
# -------------------------
|
||||
# Joint velocity constraints
|
||||
# -------------------------
|
||||
|
||||
dq_limit = 0.05 # rad per iteration
|
||||
|
||||
lb = -dq_limit * np.ones(7)
|
||||
ub = dq_limit * np.ones(7)
|
||||
|
||||
# -------------------------
|
||||
# Joint position constraints
|
||||
# -------------------------
|
||||
|
||||
q_min_step = self.model.lowerPositionLimit[:7] - q[:7]
|
||||
q_max_step = self.model.upperPositionLimit[:7] - q[:7]
|
||||
|
||||
lb = np.maximum(lb, q_min_step)
|
||||
ub = np.minimum(ub, q_max_step)
|
||||
|
||||
# -------------------------
|
||||
# Solve QP
|
||||
# ------------------------
|
||||
# Update solver
|
||||
self.osqp_solver.update(
|
||||
Px=H_triu.data,
|
||||
q=g,
|
||||
l=lb,
|
||||
u=ub
|
||||
)
|
||||
|
||||
# Solve
|
||||
result = self.osqp_solver.solve()
|
||||
|
||||
if result.info.status != 'solved':
|
||||
break
|
||||
|
||||
dq = result.x
|
||||
|
||||
if dq is None:
|
||||
break
|
||||
|
||||
# Apply joint limits with scaling
|
||||
alpha = 0.5
|
||||
q = pin.integrate(self.model, q, alpha * dq)
|
||||
|
||||
prev_error = error_norm
|
||||
iter_count += 1
|
||||
|
||||
if best_solution is not None:
|
||||
return best_solution, True, best_error
|
||||
else:
|
||||
return None, False, None
|
||||
|
||||
# def invese_kinematics_velocity(self, target_position, target_rpy=None,
|
||||
# target_quat=None, initial_guess=None, tool="ee"):
|
||||
# """
|
||||
# Compute the converging velocity (motion direction) of joints based on qp inverse kinematics.
|
||||
#
|
||||
# Args:
|
||||
# target_position: [x, y, z] target position (meters)
|
||||
# target_rpy: [roll, pitch, yaw] target orientation (radians)
|
||||
# target_quat: [x, y, z, w] target orientation as quaternion
|
||||
# initial_guess: Initial joint angles (radians)
|
||||
# tool: the frame name ('scissor', 'camera', 'ee')
|
||||
#
|
||||
# Returns:
|
||||
# joint_velocity: np.array()
|
||||
# """
|
||||
# # Build target SE3 placement
|
||||
# if target_quat is not None:
|
||||
# quat = pin.Quaternion(target_quat[3], target_quat[0],
|
||||
# target_quat[1], target_quat[2])
|
||||
# target_rotation = quat.matrix()
|
||||
# elif target_rpy is not None:
|
||||
# target_rotation = pin.rpy.rpyToMatrix(target_rpy[0],
|
||||
# target_rpy[1],
|
||||
# target_rpy[2])
|
||||
# else:
|
||||
# target_rotation = np.eye(3)
|
||||
#
|
||||
# target_placement = pin.SE3(target_rotation, np.array(target_position))
|
||||
#
|
||||
# # Try multiple initial guesses
|
||||
# initial_guesses = []
|
||||
#
|
||||
# if initial_guess is not None:
|
||||
# initial_guesses.append(initial_guess)
|
||||
# else:
|
||||
# # Try different initial configurations
|
||||
# initial_guesses.append([0.1] * 7) # Zero config
|
||||
# initial_guesses.append([radians(30), radians(45), radians(30),
|
||||
# radians(-45), radians(30), radians(-30), 0])
|
||||
# initial_guesses.append([radians(-30), radians(45), radians(-30),
|
||||
# radians(45), radians(30), radians(30), 0])
|
||||
#
|
||||
# best_solution = None
|
||||
# best_error = float('inf')
|
||||
#
|
||||
# for guess_idx, guess in enumerate(initial_guesses):
|
||||
# q = pin.neutral(self.model)
|
||||
# for i, angle in enumerate(guess):
|
||||
# if i < len(q):
|
||||
# q[i] = np.clip(angle, self.model.lowerPositionLimit[i],
|
||||
# self.model.upperPositionLimit[i])
|
||||
#
|
||||
# # Differential IK with adaptive damping
|
||||
# damping = 0.01
|
||||
# damping_reduction = 0.95
|
||||
# iter_count = 0
|
||||
# prev_error = float('inf')
|
||||
#
|
||||
# ee_frame_id = self.tool_frames[tool]
|
||||
#
|
||||
# J = pin.computeFrameJacobian(
|
||||
# self.model,
|
||||
# self.data,
|
||||
# q,
|
||||
# ee_frame_id,
|
||||
# pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
|
||||
# )
|
||||
#
|
||||
# while iter_count < max_iter:
|
||||
# # Compute forward kinematics
|
||||
#
|
||||
# pin.computeJointJacobians(self.model, self.data, q)
|
||||
# pin.framesForwardKinematics(self.model, self.data, q)
|
||||
#
|
||||
# # Get current end-effector placement
|
||||
#
|
||||
# current_placement = self.data.oMf[ee_frame_id]
|
||||
#
|
||||
# # Compute error
|
||||
# error_SE3 = current_placement.actInv(target_placement)
|
||||
# error_vec = pin.log(error_SE3).vector
|
||||
# error_norm = np.linalg.norm(error_vec)
|
||||
#
|
||||
# if error_norm < tolerance:
|
||||
# joint_angles = q[:7].copy()
|
||||
# fk_result = self.forward_kinematics(joint_angles, tool=tool)
|
||||
# position_error = np.linalg.norm(fk_result['position'] - np.array(target_position))
|
||||
#
|
||||
# if position_error < best_error:
|
||||
# best_error = position_error
|
||||
# best_solution = joint_angles
|
||||
# break
|
||||
#
|
||||
# # Check if error is increasing (diverging)
|
||||
# if error_norm > prev_error * 1.1 and iter_count > 10:
|
||||
# damping = min(1.0, damping * 1.5)
|
||||
# else:
|
||||
# damping = max(0.01, damping * damping_reduction)
|
||||
#
|
||||
# J = pin.getFrameJacobian(
|
||||
# self.model,
|
||||
# self.data,
|
||||
# ee_frame_id,
|
||||
# pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
|
||||
# )
|
||||
#
|
||||
# # =========================
|
||||
# # QP-based IK
|
||||
# # =========================
|
||||
#
|
||||
# H = J.T @ self.W @ J
|
||||
# H += damping * damping * np.eye(7)
|
||||
#
|
||||
# H_triu = sparse.triu(H).tocsc()
|
||||
#
|
||||
# g = -J.T @ self.W @ error_vec
|
||||
#
|
||||
# # -------------------------
|
||||
# # Joint velocity constraints
|
||||
# # -------------------------
|
||||
#
|
||||
# dq_limit = 0.05 # rad per iteration
|
||||
#
|
||||
# lb = -dq_limit * np.ones(7)
|
||||
# ub = dq_limit * np.ones(7)
|
||||
#
|
||||
# # -------------------------
|
||||
# # Joint position constraints
|
||||
# # -------------------------
|
||||
#
|
||||
# q_min_step = self.model.lowerPositionLimit[:7] - q[:7]
|
||||
# q_max_step = self.model.upperPositionLimit[:7] - q[:7]
|
||||
#
|
||||
# lb = np.maximum(lb, q_min_step)
|
||||
# ub = np.minimum(ub, q_max_step)
|
||||
#
|
||||
# # -------------------------
|
||||
# # Solve QP
|
||||
# # ------------------------
|
||||
# # Update solver
|
||||
# self.osqp_solver.update(
|
||||
# Px=H_triu.data,
|
||||
# q=g,
|
||||
# l=lb,
|
||||
# u=ub
|
||||
# )
|
||||
#
|
||||
# # Solve
|
||||
# result = self.osqp_solver.solve()
|
||||
#
|
||||
# if result.info.status != 'solved':
|
||||
# break
|
||||
#
|
||||
# dq = result.x
|
||||
#
|
||||
# if dq is None:
|
||||
# break
|
||||
#
|
||||
# # Apply joint limits with scaling
|
||||
# alpha = 0.5
|
||||
# q = pin.integrate(self.model, q, alpha * dq)
|
||||
#
|
||||
# prev_error = error_norm
|
||||
# iter_count += 1
|
||||
#
|
||||
# if best_solution is not None:
|
||||
# return best_solution, True, best_error
|
||||
# else:
|
||||
# return None, False, None
|
||||
|
||||
def compute_jacobian(self, joint_angles, tool="ee"):
|
||||
"""Compute geometric Jacobian (6x7)"""
|
||||
q = pin.neutral(self.model)
|
||||
@ -557,7 +602,7 @@ class KinematicsSolver():
|
||||
self.model,
|
||||
self.data,
|
||||
frame_id,
|
||||
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
|
||||
pin.ReferenceFrame.LOCAL
|
||||
)
|
||||
Js.append(J[:, active_joints])
|
||||
|
||||
|
||||
Reference in New Issue
Block a user