You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
Hello, I am able to do IK for an existing frame, but somehow not for a new frame I added. My suspicion is that I am not updating the new frame correctly.
Here is my code:
# imports
import pinocchio as pin
from pinocchio.visualize import MeshcatVisualizer
import numpy as np
from scipy.spatial.transform import Rotation as R
from numpy.linalg import norm, solve
# initialize model and add frame
model, geom_model, visual_model = pin.buildModelsFromUrdf('stretch/stretch-re2-2025/exported_urdf/stretch.urdf', 'stretch/stretch-re2-2025/exported_urdf/')
frame = pin.Frame(
name="joint_gripper_fingertip_center",
parent_joint=11, # model.parents[-2] = model.parents[-3] = 11 (parents of the right and left fingertip joints)
parent_frame=81, # model.frames[81].name = 'joint_gripper_fingertip_right'
placement=pin.SE3(
rotation=np.eye(3),
translation=np.array([0, 0, 0]),
),
type=pin.FrameType.OP_FRAME,
)
model.addFrame(frame)
data, geom_data, visual_data = pin.createDatas(model, geom_model, visual_model)
# IK function
def inverse_kinematics_stretch(pos, rot, frame, eps=1e-3, DT=1e-1, damp=1e-12):
q = pin.neutral(model)
oMdes = pin.SE3(rot, pos)
low = np.array([-1.01, -1.01, -3.9, -1.53, 0., 0., 0., 0., 0., -1.75, -1.57, -3.14, -0.6, -0.6, -1.01, -1.01]) + 1e-5
high = np.array([1.01, 1.01, 1.5, 0.79, 1.1, 0.13, 0.13, 0.13, 0.13, 4., 0.56, 3.14, 0.6 , 0.6 , 1.01, 1.01]) - 1e-5
i=0
while True:
pin.forwardKinematics(model, data, q)
pin.framesForwardKinematics(model, data, q)
# do an updateFramePlacements(model,data,q) // it will update all the oMf quantities
pin.computeJointJacobians(model,data,q)
pin.updateFramePlacements(model,data)
if frame == 'right':
frame_id = model.getFrameId('joint_gripper_fingertip_right')
else:
frame_id = model.getFrameId('joint_gripper_fingertip_center')
# update frame.placement of the frame with name "joint_gripper_fingertip_center"
data.oMf[frame_id].translation = (data.oMf[model.getFrameId('joint_gripper_fingertip_right')].translation + data.oMf[model.getFrameId('joint_gripper_fingertip_left')].translation) / 2
data.oMf[frame_id].rotation = data.oMf[65].rotation
dMi = oMdes.actInv(data.oMf[frame_id])
err = pin.log(dMi).vector
if norm(err) < eps:
success = True
break
if i >= 5000:
success = False
break
# compute your frame Jacobian.
J = pin.getFrameJacobian(model,data,frame_id,pin.LOCAL)
v = - J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))
q = pin.integrate(model,q,v*DT)
q = np.clip(q, low, high) # enforce joint limits
q[:4] = np.array([1., 0., 0., 0.]) # fix other unused joints
i += 1
center_trans = (data.oMf[model.getFrameId('joint_gripper_fingertip_right')].translation + data.oMf[model.getFrameId('joint_gripper_fingertip_left')].translation) / 2
center_rot = data.oMf[65].rotation
print(norm(err))
return q, success, center_trans, center_rot
Now, IK works flawlessly when I do it for the joint_gripper_fingertip_right frame, which existed beforehand:
However, now when I try to do IK for my new frame (joint_gripper_fingertip_center), with the target pose being the pose of the joint_gripper_fingertip_center frame in the 10 sets of joint angles found above (in this way, we know that these poses are achievable for this frame), the IK does not converge:
for i in range(10):
q, success, _, _ = inverse_kinematics_stretch(center_trans_arr[i], center_rot_arr[i], 'center')
# OUTPUT:
# 0.21691378853034962
# 0.23799450520792612
# 0.2567126211275081
# 0.28118571952270865
# 0.23561804504528602
# 0.1873477635739255
# 0.15636521831159536
# 0.09449267405432853
# 0.047944524632797705
# 0.5051757439769025
All of the errors are (much) larger than 0.01, and IK does not find a good solution.
As another sanity check, I tried doing IK for the joint_gripper_fingertip_center frame in the neutral position, which works:
# get joint_gripper_fingertip_center pose for neutral position
q = pin.neutral(model)
pin.forwardKinematics(model, data, q)
pin.framesForwardKinematics(model, data, q)
# do an updateFramePlacements(model,data,q) // it will update all the oMf quantities
pin.computeJointJacobians(model,data,q)
pin.updateFramePlacements(model,data)
center_trans_neutral = (data.oMf[model.getFrameId('joint_gripper_fingertip_right')].translation + data.oMf[model.getFrameId('joint_gripper_fingertip_left')].translation) / 2
center_rot_neutral = data.oMf[65].rotation # R.from_matrix(data.oMf[65].rotation).as_euler('xyz')
q, success, _, _ = inverse_kinematics_stretch(center_trans_neutral, center_rot_neutral, 'center')
# OUTPUT:
# 0.0
However, if I perturb the target position by just a little bit, IK is unable to find a solution:
reacted with thumbs up emoji reacted with thumbs down emoji reacted with laugh emoji reacted with hooray emoji reacted with confused emoji reacted with heart emoji reacted with rocket emoji reacted with eyes emoji
-
Hello, I am able to do IK for an existing frame, but somehow not for a new frame I added. My suspicion is that I am not updating the new frame correctly.
Here is my code:
Now, IK works flawlessly when I do it for the
joint_gripper_fingertip_right
frame, which existed beforehand:However, now when I try to do IK for my new frame (
joint_gripper_fingertip_center
), with the target pose being the pose of thejoint_gripper_fingertip_center
frame in the 10 sets of joint angles found above (in this way, we know that these poses are achievable for this frame), the IK does not converge:All of the errors are (much) larger than 0.01, and IK does not find a good solution.
As another sanity check, I tried doing IK for the
joint_gripper_fingertip_center
frame in the neutral position, which works:However, if I perturb the target position by just a little bit, IK is unable to find a solution:
Any ideas what I might be doing wrong here?
Any help will be much appreciated, thanks in advance!
Beta Was this translation helpful? Give feedback.
All reactions