add ik q_solved refinement function, to make it as close as possible to last_q.
update the rm75_kine_rm.py to included a series of arm-angle for inverse kinematics calculation.
This commit is contained in:
LiuzhengSJ
+441
-224
@@ -26,48 +26,116 @@ class KinematicsSolver():
|
||||
self.geom_model.addAllCollisionPairs()
|
||||
self.remove_adjacent_collision_pairs(verbose=True)
|
||||
self.geom_data = pin.GeometryData(self.geom_model)
|
||||
self.data = self.model.createData()
|
||||
|
||||
|
||||
self.cfg_j_limit()
|
||||
|
||||
q_range = ( self.model.upperPositionLimit[:7] - self.model.lowerPositionLimit[:7] )
|
||||
self.nv = 7
|
||||
q_range = ( self.model.upperPositionLimit[:self.nv] - self.model.lowerPositionLimit[:self.nv] )
|
||||
|
||||
self.w_q_limit = np.diag(1.0 / (q_range ** 2))
|
||||
|
||||
self.q_mid = 0.5 * (self.model.lowerPositionLimit[:7] + self.model.upperPositionLimit[:7])
|
||||
self.q_mid = 0.5 * (self.model.lowerPositionLimit[:self.nv] + self.model.upperPositionLimit[:self.nv])
|
||||
|
||||
# ---------- for reused qp_solver ------------------
|
||||
self.nv = 7
|
||||
# ---------------------------------------------------------
|
||||
# Primary IK solver
|
||||
#
|
||||
# Optimization variable:
|
||||
# dq in R^7
|
||||
#
|
||||
# Constraints:
|
||||
# lb <= dq <= ub
|
||||
#
|
||||
# Therefore:
|
||||
# A = I, shape = 7 x 7
|
||||
|
||||
# Full dense symmetric matrix structure
|
||||
# P_template = np.triu(np.ones((7, 7)))
|
||||
self.P_pattern = sparse.triu(np.ones((7,7))).tocsc()
|
||||
|
||||
P_sparse = sparse.csc_matrix(self.P_pattern)
|
||||
|
||||
A_sparse = sparse.eye(7, format='csc')
|
||||
|
||||
self.ik_P_pattern = sparse.triu( np.ones((self.nv, self.nv)) ).tocsc()
|
||||
self.osqp_solver = osqp.OSQP()
|
||||
|
||||
self.osqp_solver.setup(
|
||||
P=P_sparse,
|
||||
q=np.zeros(7),
|
||||
A=A_sparse,
|
||||
l=-np.ones(7),
|
||||
u=np.ones(7),
|
||||
P=self.ik_P_pattern,
|
||||
q=np.zeros(self.nv),
|
||||
A=sparse.eye(self.nv, format='csc'),
|
||||
l=-np.ones(self.nv),
|
||||
u=np.ones(self.nv),
|
||||
verbose=False,
|
||||
warm_start=True,
|
||||
polish=False
|
||||
)
|
||||
|
||||
# End-effector task weight:
|
||||
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
|
||||
# Larger value => joint moves more lazy
|
||||
self.joint_motion_weight = np.diag([
|
||||
1.0, 1.0, 1.0, 1.0,
|
||||
0.3, 0.3, 0.2
|
||||
])
|
||||
|
||||
# ---------------------------------------------------------
|
||||
# Refinement solver
|
||||
#
|
||||
# Constraints:
|
||||
#
|
||||
# J_eff * dq = error_vec 6 constraints
|
||||
# dq_lower <= dq <= dq_upper 7 constraints
|
||||
#
|
||||
# A_refine = [J_eff]
|
||||
# [ I ]
|
||||
#
|
||||
# Shape:
|
||||
# 13 x 7
|
||||
#
|
||||
# The upper 6 x 7 block must have a dense sparsity
|
||||
# pattern because every Jacobian entry can change.
|
||||
# ---------------------------------------------------------
|
||||
|
||||
# The refinement Hessian used below is diagonal:
|
||||
#
|
||||
# H = w_last * W_last
|
||||
# + w_mid * W_mid
|
||||
# + damping * I
|
||||
#
|
||||
# Therefore a diagonal P pattern is sufficient.
|
||||
self.refine_P_pattern = sparse.eye(
|
||||
self.nv,
|
||||
format="csc",
|
||||
)
|
||||
|
||||
# Dense structural pattern for the 6x7 Jacobian block.
|
||||
refine_J_pattern = sparse.csc_matrix(
|
||||
np.ones((6, self.nv))
|
||||
)
|
||||
|
||||
# Identity pattern for joint-step bounds.
|
||||
refine_I_pattern = sparse.eye(
|
||||
self.nv,
|
||||
format="csc",
|
||||
)
|
||||
|
||||
self.refine_A_pattern = sparse.vstack(
|
||||
[
|
||||
refine_J_pattern,
|
||||
refine_I_pattern,
|
||||
],
|
||||
format="csc",
|
||||
)
|
||||
|
||||
self.refine_osqp_solver = osqp.OSQP()
|
||||
|
||||
self.refine_osqp_solver.setup(
|
||||
P=self.refine_P_pattern,
|
||||
q=np.zeros(self.nv),
|
||||
A=self.refine_A_pattern,
|
||||
l=-np.ones(6 + self.nv),
|
||||
u=np.ones(6 + self.nv),
|
||||
verbose=False,
|
||||
warm_start=True,
|
||||
polish=False,
|
||||
)
|
||||
|
||||
if tcps is None:
|
||||
pass
|
||||
@@ -174,7 +242,7 @@ class KinematicsSolver():
|
||||
debug: Print debug information
|
||||
tool: the frame name ('scissor', 'camera', 'ee')
|
||||
Returns:
|
||||
tuple: (joint_angles, success, error)
|
||||
sts, q_solved
|
||||
"""
|
||||
# Build target SE3 placement
|
||||
if target_quat is not None:
|
||||
@@ -241,9 +309,6 @@ class KinematicsSolver():
|
||||
error_SE3 = current_placement.actInv(target_placement)
|
||||
error_vec = pin.log(error_SE3).vector
|
||||
|
||||
# print("\n initial error =", np.linalg.norm(error_vec))
|
||||
# print(error_vec)
|
||||
|
||||
while iter_count < max_iter:
|
||||
# Compute forward kinematics
|
||||
|
||||
@@ -357,6 +422,360 @@ class KinematicsSolver():
|
||||
# return q[:7].copy(), False, error_norm, iter_count
|
||||
return -1, q[:7].copy().tolist()
|
||||
|
||||
def refine_ik_solution(
|
||||
self,
|
||||
q_valid,
|
||||
q_last,
|
||||
tool="ee",
|
||||
max_iter=30,
|
||||
pose_tolerance=5e-4,
|
||||
joint_tolerance=1e-5,
|
||||
step_limit=None,
|
||||
w_last=1.0,
|
||||
w_mid=0.0001,
|
||||
damping=1e-5,
|
||||
debug=False,
|
||||
):
|
||||
"""
|
||||
Refine an already-valid IK solution.
|
||||
|
||||
The initial configuration q_valid already reaches the desired
|
||||
end-effector pose. The function searches for another configuration
|
||||
that:
|
||||
|
||||
1. maintains the end-effector pose generated by q_valid;
|
||||
2. is closer to q_last;
|
||||
3. optionally remains away from joint limits.
|
||||
|
||||
Optimization at each iteration:
|
||||
|
||||
minimize over dq:
|
||||
|
||||
0.5 * w_last *
|
||||
||q + dq - q_last||^2_{W_last}
|
||||
|
||||
+ 0.5 * w_mid *
|
||||
||q + dq - q_mid||^2_{W_mid}
|
||||
|
||||
+ 0.5 * damping * ||dq||^2
|
||||
|
||||
subject to:
|
||||
|
||||
J_eff(q) dq = error_vec(q)
|
||||
|
||||
dq_lower <= dq <= dq_upper
|
||||
|
||||
The pose error is defined relative to the pose generated by q_valid.
|
||||
|
||||
Returns:
|
||||
status, q_refined
|
||||
|
||||
Status:
|
||||
0: refinement completed successfully
|
||||
-1: invalid input or tool
|
||||
-2: refinement QP failed
|
||||
-3: final pose error exceeds tolerance
|
||||
-4: refined configuration is in collision
|
||||
"""
|
||||
|
||||
# ---------------------------------------------------------
|
||||
# Validate input
|
||||
# ---------------------------------------------------------
|
||||
q = np.asarray(q_valid, dtype=np.float64).reshape(-1)
|
||||
q_last = np.asarray(q_last, dtype=np.float64).reshape(-1)
|
||||
|
||||
if q.size != self.nv:
|
||||
if debug:
|
||||
print(f"q_valid must contain {self.nv} values, " f"got {q.size}" )
|
||||
return -1, q.tolist()
|
||||
|
||||
if q_last.size != self.nv:
|
||||
if debug:
|
||||
print( f"q_last must contain {self.nv} values, " f"got {q_last.size}" )
|
||||
return -1, q.tolist()
|
||||
|
||||
q_lower = self.model.lowerPositionLimit[:self.nv]
|
||||
q_upper = self.model.upperPositionLimit[:self.nv]
|
||||
|
||||
q = np.clip(q, q_lower, q_upper)
|
||||
q_last = np.clip(q_last, q_lower, q_upper)
|
||||
|
||||
# ---------------------------------------------------------
|
||||
# Resolve end-effector frame
|
||||
# ---------------------------------------------------------
|
||||
if tool in self.tcps:
|
||||
tcp_index = self.tcps.index(tool)
|
||||
ee_frame_id = self.tcp_ids[tcp_index]
|
||||
else:
|
||||
try:
|
||||
ee_frame_id = self.tool_frames[tool]
|
||||
except:
|
||||
print(f'{tool} definition not found')
|
||||
|
||||
|
||||
# ---------------------------------------------------------
|
||||
# Set per-joint refinement step limits
|
||||
# ---------------------------------------------------------
|
||||
if step_limit is None:
|
||||
dq_limit = np.array([ 0.02, 0.02, 0.02, 0.02, 0.03, 0.03, 0.04, ])
|
||||
else:
|
||||
dq_limit = np.asarray( step_limit, dtype=np.float64, )
|
||||
|
||||
if dq_limit.ndim == 0:
|
||||
dq_limit = np.full( self.nv, float(dq_limit),)
|
||||
else:
|
||||
dq_limit = dq_limit.reshape(-1)
|
||||
|
||||
if dq_limit.size != self.nv:
|
||||
if debug:
|
||||
print( f"step_limit must be scalar or contain " f"{self.nv} values")
|
||||
return -1, q.tolist()
|
||||
|
||||
if np.any(dq_limit <= 0.0):
|
||||
if debug:
|
||||
print("All step limits must be positive")
|
||||
return -1, q.tolist()
|
||||
|
||||
# ---------------------------------------------------------
|
||||
# Save the end-effector pose produced by q_valid
|
||||
# ---------------------------------------------------------
|
||||
pin.forwardKinematics( self.model, self.data, q, )
|
||||
pin.updateFramePlacements( self.model, self.data, )
|
||||
|
||||
target_placement = self.data.oMf[ee_frame_id].copy()
|
||||
|
||||
# ---------------------------------------------------------
|
||||
# Objective weight matrices
|
||||
# ---------------------------------------------------------
|
||||
|
||||
# Temporal continuity:
|
||||
# Larger weight for a joint means that joint is more strongly
|
||||
# encouraged to remain close to q_last.
|
||||
W_last = np.eye(self.nv)
|
||||
|
||||
# Joint-limit-centering weight.
|
||||
W_mid = self.w_q_limit
|
||||
|
||||
# Since all objective matrices are diagonal, H is diagonal.
|
||||
H_diag = ( w_last * np.diag(W_last) + w_mid * np.diag(W_mid) + damping * np.ones(self.nv) )
|
||||
|
||||
# OSQP uses:
|
||||
# 0.5 dq.T P dq + g.T dq
|
||||
# P values correspond to the diagonal pattern created in __init__.
|
||||
Px = H_diag.copy()
|
||||
|
||||
# Reset the previous refinement warm start.
|
||||
self.refine_osqp_solver.warm_start( x=np.zeros(self.nv) )
|
||||
|
||||
previous_distance = np.linalg.norm(q - q_last)
|
||||
|
||||
# ---------------------------------------------------------
|
||||
# Sequential refinement loop
|
||||
# ---------------------------------------------------------
|
||||
for iteration in range(max_iter):
|
||||
|
||||
# Update kinematics and frame Jacobians.
|
||||
pin.computeJointJacobians( self.model, self.data, q, )
|
||||
|
||||
pin.framesForwardKinematics( self.model, self.data, q, )
|
||||
|
||||
current_placement = self.data.oMf[ee_frame_id]
|
||||
|
||||
# Pose error relative to the pose saved from q_valid.
|
||||
error_SE3 = current_placement.actInv( target_placement )
|
||||
|
||||
error_vec = pin.log(error_SE3).vector
|
||||
pose_error_norm = np.linalg.norm(error_vec)
|
||||
|
||||
J = pin.getFrameJacobian(
|
||||
self.model,
|
||||
self.data,
|
||||
ee_frame_id,
|
||||
pin.ReferenceFrame.LOCAL,
|
||||
)
|
||||
|
||||
J_eff = pin.Jlog6(error_SE3) @ J
|
||||
|
||||
# -----------------------------------------------------
|
||||
# Linear objective vector
|
||||
# -----------------------------------------------------
|
||||
# 0.5*w_last*||q+dq-q_last||^2_Wlast
|
||||
# the linear term is:
|
||||
# w_last*W_last*(q-q_last)
|
||||
# The same expansion applies to q_mid.
|
||||
# -----------------------------------------------------
|
||||
g = (w_last * W_last @ (q - q_last) + w_mid * W_mid @ (q - self.q_mid) )
|
||||
|
||||
# -----------------------------------------------------
|
||||
# Pose equality constraint
|
||||
# -----------------------------------------------------
|
||||
# e(q + dq) approximately equals:
|
||||
# e(q) - J_eff dq
|
||||
# Requiring the next error to be zero gives:
|
||||
# J_eff dq = e(q)
|
||||
# -----------------------------------------------------
|
||||
pose_lower = error_vec.copy()
|
||||
pose_upper = error_vec.copy()
|
||||
|
||||
# -----------------------------------------------------
|
||||
# Joint increment and position constraints
|
||||
# -----------------------------------------------------
|
||||
joint_lower = np.maximum(
|
||||
-dq_limit,
|
||||
q_lower - q,
|
||||
)
|
||||
|
||||
joint_upper = np.minimum(
|
||||
dq_limit,
|
||||
q_upper - q,
|
||||
)
|
||||
|
||||
lower = np.concatenate([
|
||||
pose_lower,
|
||||
joint_lower,
|
||||
])
|
||||
|
||||
upper = np.concatenate([
|
||||
pose_upper,
|
||||
joint_upper,
|
||||
])
|
||||
|
||||
# -----------------------------------------------------
|
||||
# Update A numerical values
|
||||
# -----------------------------------------------------
|
||||
# refine_A_pattern is CSC. For every joint/column j,
|
||||
# its stored entries are:
|
||||
# J_eff[0, j]
|
||||
# J_eff[1, j]
|
||||
# ...
|
||||
# J_eff[5, j]
|
||||
# I[j, j] = 1
|
||||
# This produces 7 values per column and 49 total.
|
||||
# -----------------------------------------------------
|
||||
Ax = np.concatenate([
|
||||
np.concatenate([
|
||||
J_eff[:, joint_index],
|
||||
np.array([1.0]),
|
||||
])
|
||||
for joint_index in range(self.nv)
|
||||
])
|
||||
|
||||
self.refine_osqp_solver.update(
|
||||
Px=Px,
|
||||
q=g,
|
||||
Ax=Ax,
|
||||
l=lower,
|
||||
u=upper,
|
||||
)
|
||||
|
||||
result = self.refine_osqp_solver.solve()
|
||||
|
||||
if result.info.status not in (
|
||||
"solved",
|
||||
"solved inaccurate",
|
||||
):
|
||||
if debug:
|
||||
print(
|
||||
"Refinement QP failed at iteration "
|
||||
f"{iteration}: {result.info.status}"
|
||||
)
|
||||
return -2, q.tolist()
|
||||
|
||||
dq = result.x
|
||||
|
||||
if dq is None or not np.all(np.isfinite(dq)):
|
||||
if debug:
|
||||
print(
|
||||
f"Invalid refinement result at iteration "
|
||||
f"{iteration}"
|
||||
)
|
||||
return -2, q.tolist()
|
||||
|
||||
dq_norm = np.linalg.norm(dq)
|
||||
|
||||
if debug:
|
||||
distance_to_last = np.linalg.norm(q - q_last)
|
||||
|
||||
print(
|
||||
f"refine iteration={iteration:02d}, "
|
||||
f"pose_error={pose_error_norm:.8f}, "
|
||||
f"distance_to_last={distance_to_last:.6f}, "
|
||||
f"dq_norm={dq_norm:.8f}"
|
||||
)
|
||||
|
||||
# No useful redundant motion remains.
|
||||
if dq_norm < joint_tolerance and pose_error_norm < pose_tolerance:
|
||||
break
|
||||
|
||||
q_candidate = pin.integrate( self.model, q, dq, )
|
||||
|
||||
q_candidate = np.clip( q_candidate, q_lower, q_upper, )
|
||||
|
||||
candidate_distance = np.linalg.norm( q_candidate - q_last )
|
||||
|
||||
# The pose-correction component can occasionally make the
|
||||
# distance increase slightly. Permit a tiny numerical margin.
|
||||
if candidate_distance> previous_distance + 1e-8 and pose_error_norm < pose_tolerance:
|
||||
if debug:
|
||||
print( "Refinement stopped because the candidate " "does not improve temporal continuity" )
|
||||
break
|
||||
|
||||
q = q_candidate
|
||||
previous_distance = candidate_distance
|
||||
|
||||
# ---------------------------------------------------------
|
||||
# Final pose verification
|
||||
# ---------------------------------------------------------
|
||||
pin.forwardKinematics( self.model, self.data, q, )
|
||||
|
||||
pin.updateFramePlacements( self.model, self.data, )
|
||||
|
||||
final_placement = self.data.oMf[ee_frame_id]
|
||||
|
||||
final_error_SE3 = final_placement.actInv( target_placement )
|
||||
|
||||
final_error_vec = pin.log( final_error_SE3 ).vector
|
||||
|
||||
final_pose_error = np.linalg.norm( final_error_vec )
|
||||
|
||||
if debug:
|
||||
print(
|
||||
f"Final refinement pose error: "
|
||||
f"{final_pose_error:.8f}"
|
||||
)
|
||||
print(
|
||||
f"Original distance to last: "
|
||||
f"{np.linalg.norm(np.asarray(q_valid) - q_last):.6f}"
|
||||
)
|
||||
print(
|
||||
f"Refined distance to last: "
|
||||
f"{np.linalg.norm(q - q_last):.6f}"
|
||||
)
|
||||
|
||||
if final_pose_error > pose_tolerance:
|
||||
if debug:
|
||||
print(
|
||||
"Refined solution rejected because its "
|
||||
"pose error is too large"
|
||||
)
|
||||
return -3, np.asarray(q_valid).tolist()
|
||||
|
||||
# ---------------------------------------------------------
|
||||
# Final collision verification
|
||||
# ---------------------------------------------------------
|
||||
collision = self.collision_detect( q=q, stop_at_first_collision=True, )
|
||||
|
||||
if collision:
|
||||
if debug:
|
||||
print(
|
||||
"Refined solution rejected because it is "
|
||||
"in collision"
|
||||
)
|
||||
return -4, np.asarray(q_valid).tolist()
|
||||
|
||||
return 0, q.tolist()
|
||||
|
||||
|
||||
def collision_detect(self, q ,stop_at_first_collision=True, verbose=False ):
|
||||
q = np.asarray(q, dtype=np.float64).reshape(-1)
|
||||
|
||||
@@ -510,10 +929,7 @@ class KinematicsSolver():
|
||||
|
||||
return J
|
||||
|
||||
def get_subchain_jacobian(self,
|
||||
joint_angles,
|
||||
frame_names
|
||||
):
|
||||
def get_subchain_jacobian(self, joint_angles, frame_names ):
|
||||
|
||||
q = pin.neutral(self.model)
|
||||
|
||||
@@ -560,7 +976,6 @@ class KinematicsSolver():
|
||||
|
||||
active_joint_ids = []
|
||||
|
||||
|
||||
# Traverse upward to root
|
||||
while joint_id > 0:
|
||||
# Pinocchio joint indexing:
|
||||
@@ -577,201 +992,3 @@ class KinematicsSolver():
|
||||
all_active_joint_ids.append(active_joint_ids)
|
||||
|
||||
return all_active_joint_ids
|
||||
|
||||
def plan_cartesian_trajectory(self, start_pos, end_pos,
|
||||
start_rpy=None, end_rpy=None,
|
||||
num_steps=20, tool='ee'):
|
||||
"""
|
||||
Plan a Cartesian trajectory with IK for each waypoint.
|
||||
"""
|
||||
# Get current end-effector pose if start_rpy not provided
|
||||
if start_rpy is None:
|
||||
# Try to find a valid starting configuration
|
||||
test_angles = [0.1] * 7
|
||||
fk_test = self.forward_kinematics(test_angles,tool=tool)
|
||||
start_rpy = fk_test['rpy']
|
||||
|
||||
if end_rpy is None:
|
||||
end_rpy = start_rpy
|
||||
|
||||
# First, check if target is reachable
|
||||
print(f"\nChecking if target is reachable...")
|
||||
target_pos = end_pos
|
||||
target_rpy = end_rpy
|
||||
|
||||
test_solution, success, error = self.inverse_kinematics(
|
||||
target_pos, target_rpy=target_rpy, initial_guess=[0.1] * 7, max_iter=500, tool=tool
|
||||
)
|
||||
|
||||
if not success:
|
||||
print(f"Warning: Target may be unreachable or difficult to reach")
|
||||
print(f"Trying with relaxed tolerance...")
|
||||
|
||||
# Initial guess for IK (start with zero configuration)
|
||||
current_angles = [0.1] * 7
|
||||
trajectory = []
|
||||
|
||||
print(f"\nPlanning trajectory from ({start_pos[0]:.2f}, {start_pos[1]:.2f}, {start_pos[2]:.2f})")
|
||||
print(f"To ({end_pos[0]:.2f}, {end_pos[1]:.2f}, {end_pos[2]:.2f})")
|
||||
print("-" * 60)
|
||||
|
||||
for i in range(num_steps + 1):
|
||||
t = i / num_steps
|
||||
|
||||
# Interpolate position
|
||||
pos = [
|
||||
start_pos[0] + t * (end_pos[0] - start_pos[0]),
|
||||
start_pos[1] + t * (end_pos[1] - start_pos[1]),
|
||||
start_pos[2] + t * (end_pos[2] - start_pos[2])
|
||||
]
|
||||
|
||||
# Interpolate orientation
|
||||
rpy = [
|
||||
start_rpy[0] + t * (end_rpy[0] - start_rpy[0]),
|
||||
start_rpy[1] + t * (end_rpy[1] - start_rpy[1]),
|
||||
start_rpy[2] + t * (end_rpy[2] - start_rpy[2])
|
||||
]
|
||||
|
||||
# Compute IK
|
||||
joint_angles, success, error = self.inverse_kinematics(
|
||||
pos, target_rpy=rpy, initial_guess=current_angles, max_iter=300, tool=tool
|
||||
)
|
||||
|
||||
if not success:
|
||||
print(f" Waypoint {i}: IK failed!")
|
||||
break
|
||||
|
||||
# Verify
|
||||
fk_verify = self.forward_kinematics(joint_angles, tool=tool)
|
||||
|
||||
trajectory.append({
|
||||
'step': i,
|
||||
't': t,
|
||||
'position': pos,
|
||||
'rpy': rpy,
|
||||
'joint_angles': joint_angles,
|
||||
'actual_position': fk_verify['position'],
|
||||
'error': error
|
||||
})
|
||||
|
||||
# Update current angles for next iteration
|
||||
current_angles = joint_angles
|
||||
|
||||
if i % 5 == 0 or i == num_steps:
|
||||
print(f" Waypoint {i:3d}: pos=({pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f}), "
|
||||
f"error={error:.6f}m")
|
||||
|
||||
return trajectory
|
||||
|
||||
|
||||
|
||||
def main():
|
||||
"""Main test function"""
|
||||
|
||||
|
||||
rm75 = KinematicsSolver()
|
||||
|
||||
# Test 1: Forward Kinematics
|
||||
print("\n1. Forward Kinematics Test")
|
||||
print("-" * 40)
|
||||
|
||||
tool_name = "scissor"
|
||||
joint_angles_zero = [0.1] * 7
|
||||
fk_result = rm75.forward_kinematics(joint_angles_zero, tool=tool_name)
|
||||
|
||||
print(f"Init configuration:")
|
||||
print(f" Position: ({fk_result['position'][0]:.3f}, "
|
||||
f"{fk_result['position'][1]:.3f}, {fk_result['position'][2]:.3f}) m")
|
||||
|
||||
# Test 2: Inverse Kinematics with more reachable target
|
||||
print("\n2. Inverse Kinematics Test")
|
||||
print("-" * 40)
|
||||
|
||||
# Try a simpler target first
|
||||
target_pos = [0.3, 0.2, 0.4] # More reachable position
|
||||
target_rpy = [0.0, 0.0, radians(45)] # Simpler orientation
|
||||
|
||||
print(f"Target: ({target_pos[0]:.3f}, {target_pos[1]:.3f}, {target_pos[2]:.3f}) m")
|
||||
|
||||
import time
|
||||
init_joints = [0.2] * 7
|
||||
time0 = time.time()
|
||||
for ii in range(100):
|
||||
joint_solution, success, error = rm75.inverse_kinematics(
|
||||
target_pos, target_rpy=target_rpy, initial_guess=init_joints,
|
||||
max_iter=500, debug=False, tool=tool_name
|
||||
)
|
||||
time1 = time.time()
|
||||
print(f"Time: {time1 - time0}")
|
||||
|
||||
if success:
|
||||
print(f"✓ Solution found! Error: {error:.6f} m")
|
||||
for i, angle in enumerate(joint_solution):
|
||||
print(f" Joint {i + 1}: {degrees(angle):7.2f}°")
|
||||
|
||||
# Verify
|
||||
fk_verify = rm75.forward_kinematics(joint_solution,tool=tool_name)
|
||||
print(
|
||||
f" Position: ({fk_verify['position'][0]:.3f}, {fk_verify['position'][1]:.3f}, {fk_verify['position'][2]:.3f}) m")
|
||||
else:
|
||||
print("✗ IK failed to find a solution!")
|
||||
|
||||
# Test 3: Jacobian
|
||||
print("\n3. Jacobian Matrix")
|
||||
print("-" * 40)
|
||||
|
||||
J = rm75.compute_jacobian(joint_angles_zero, tool=tool_name)
|
||||
print(f"Jacobian shape: {J.shape}")
|
||||
for i in range(min(3, J.shape[0])):
|
||||
row_str = " ".join([f"{J[i, j]:7.3f}" for j in range(7)])
|
||||
print(f" Row {i + 1}: {row_str}")
|
||||
|
||||
# Test 4: Trajectory Planning with reachable positions
|
||||
print("\n4. Cartesian Trajectory Planning")
|
||||
print("-" * 40)
|
||||
|
||||
start_pos = [0.3, 0.0, 0.4] # Start position
|
||||
end_pos = [0.3, 0.0, 0.55] # End position (smaller movement)
|
||||
|
||||
fk0 = rm75.forward_kinematics([0.1] * 7, tool=tool_name)
|
||||
|
||||
trajectory = rm75.plan_cartesian_trajectory(
|
||||
start_pos,
|
||||
end_pos,
|
||||
start_rpy=fk0['rpy'],
|
||||
end_rpy=[
|
||||
fk0['rpy'][0] + radians(10),
|
||||
fk0['rpy'][1],
|
||||
fk0['rpy'][2]
|
||||
],
|
||||
num_steps=10,
|
||||
tool=tool_name
|
||||
)
|
||||
|
||||
if trajectory:
|
||||
print(f"\n✓ Generated {len(trajectory)} waypoints")
|
||||
|
||||
if success:
|
||||
print("✓ Inverse kinematics working (with simplified target)")
|
||||
else:
|
||||
print("⚠ Inverse kinematics may need tuning - try different targets")
|
||||
|
||||
|
||||
print("\n" + "=" * 60)
|
||||
print(f'test subchain Jacobian, for future obstacle avoidance')
|
||||
frame_names = [
|
||||
"link_2",
|
||||
"link_4",
|
||||
"link_7"
|
||||
]
|
||||
Js_sub = rm75.get_subchain_jacobian(
|
||||
joint_angles=joint_angles_zero,
|
||||
frame_names=frame_names
|
||||
)
|
||||
print(f'Js_sub: {Js_sub}')
|
||||
|
||||
return rm75, trajectory
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
rm75, trajectory = main()
|
||||
|
||||
Reference in New Issue
Block a user