def cost(self, x):
tauq, fr, fl = self.x2vars(x)
pinocchio.computeAllTerms(self.rmodel, self.rdata, self.q, self.vq)
b = self.rdata.nle
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
Jr = pinocchio.getFrameJacobian(self.rmodel, self.rdata, self.idR,
LOCAL)
Jl = pinocchio.getFrameJacobian(self.rmodel, self.rdata, self.idL,
LOCAL)
self.residuals = m2a(b - tauq - Jr.T * fr - Jl.T * fl)
return sum(self.residuals**2)
def calcDiff(self, data, x, recalc=True):
if recalc:
self.calc(data, x)
dv_dq, da_dq, da_dv, da_da = pinocchio.getJointAccelerationDerivatives(
self.pinocchio, data.pinocchio, data.joint,
pinocchio.ReferenceFrame.LOCAL)
dv_dq, dv_dvq = pinocchio.getJointVelocityDerivatives(
self.pinocchio, data.pinocchio, data.joint,
pinocchio.ReferenceFrame.LOCAL)
vw = data.v.angular
vv = data.v.linear
data.Aq[:, :] = (data.fXj * da_dq)[:3, :] + skew(vw) * (
data.fXj * dv_dq)[:3, :] - skew(vv) * (data.fXj * dv_dq)[3:, :]
data.Av[:, :] = (data.fXj *
da_dv)[:3, :] + skew(vw) * data.J - skew(vv) * data.Jw
R = data.pinocchio.oMf[self.frame].rotation
if self.gains[0] != 0.:
data.Aq[:, :] += self.gains[0] * R * pinocchio.getFrameJacobian(
self.pinocchio, data.pinocchio, self.frame,
pinocchio.ReferenceFrame.LOCAL)[:3, :]
if self.gains[1] != 0.:
data.Aq[:, :] += self.gains[1] * (data.fXj[:3, :] * dv_dq)
data.Av[:, :] += self.gains[1] * (data.fXj[:3, :] * dv_dvq)
def computeJacobian(self, q):
pin.forwardKinematics(self.rmodel, self.rdata, q)
pin.updateFramePlacements(self.rmodel, self.rdata)
pin.computeJointJacobians(self.rmodel, self.rdata, q)
J = pin.getFrameJacobian(self.rmodel, self.rdata, self.ee_frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
return J
def test_frame_algo(self):
model = self.model
data = model.createData()
q = pin.neutral(model)
v = np.random.rand((model.nv))
frame_id = self.frame_idx
J1 = pin.computeFrameJacobian(model, data, q, frame_id)
J2 = pin.computeFrameJacobian(model, data, q, frame_id, pin.LOCAL)
self.assertApprox(J1, J2)
data2 = model.createData()
pin.computeJointJacobians(model, data2, q)
J3 = pin.getFrameJacobian(model, data2, frame_id, pin.LOCAL)
self.assertApprox(J1, J3)
dJ1 = pin.frameJacobianTimeVariation(model, data, q, v, frame_id,
pin.LOCAL)
data3 = model.createData()
pin.computeJointJacobiansTimeVariation(model, data3, q, v)
dJ2 = pin.getFrameJacobianTimeVariation(model, data3, frame_id,
pin.LOCAL)
self.assertApprox(dJ1, dJ2)
def calcDiff3d(self, q):
pin.forwardKinematics(robot.model, robot.data, q)
M = pin.updateFramePlacement(robot.model, robot.data, self.frameIndex)
pin.computeJointJacobians(robot.model, robot.data, q)
J = pin.getFrameJacobian(robot.model, robot.data, self.frameIndex,
pin.LOCAL_WORLD_ALIGNED)[:3, :]
return 2 * J.T @ (M.translation - self.Mtarget.translation)
def main():
'''
Main procedure
1 ) Build the model from an URDF file
2 ) compute forwardKinematics
3 ) compute forwardKinematics of the frames
4 ) explore data
'''
model_file = Path(__file__).absolute().parents[1].joinpath(
'urdf', 'twolinks.urdf')
model = buildModelFromUrdf(str(model_file))
# Create data required by the algorithms
data = model.createData()
# Sample a random configuration
numpy_vector_joint_pos = randomConfiguration(model)
# THIS FUNCTION CALLS THE FORWARD KINEMATICS
computeJointJacobians(model, data, numpy_vector_joint_pos)
joint_number = model.njoints
for i in range(joint_number):
joint = model.joints[i]
joint_placement = data.oMi[i]
joint_jacobian = getJointJacobian(model, data, i, ReferenceFrame.WORLD)
# CAUTION updateFramePlacements must be called to update frame's positions
# Remark: in pinocchio frames, joints and bodies are different things
updateFramePlacements(model, data)
# Print out the placement of each joint of the kinematic tree
frame_number = model.nframes
for i in range(frame_number):
frame = model.frames[i]
frame_placement = data.oMf[i]
frame_jacobian = getFrameJacobian(model, data, i, ReferenceFrame.WORLD)
def calcDiff(self, data, x):
v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da = pinocchio.getJointAccelerationDerivatives(
self.state.pinocchio, data.pinocchio, self.joint,
pinocchio.ReferenceFrame.LOCAL)
vv_skew = pinocchio.utils.skew(self.vv)
vw_skew = pinocchio.utils.skew(self.vw)
fXjdv_dq = self.fXj * v_partial_dq
da0_dq = (
self.fXj * a_partial_dq
)[:3, :] + vw_skew * fXjdv_dq[:3, :] - vv_skew * fXjdv_dq[3:, :]
da0_dv = (self.fXj *
a_partial_dv)[:3, :] + vw_skew * data.Jc - vv_skew * self.Jw
if np.asscalar(self.gains[0]) != 0.:
R = data.pinocchio.oMf[self.xref.frame].rotation
da0_dq += np.asscalar(
self.gains[0]) * R * pinocchio.getFrameJacobian(
self.state.pinocchio, data.pinocchio, self.xref.frame,
pinocchio.ReferenceFrame.LOCAL)[:3, :]
if np.asscalar(self.gains[1]) != 0.:
da0_dq += np.asscalar(
self.gains[1]) * (self.fXj[:3, :] * v_partial_dq)
da0_dv += np.asscalar(
self.gains[1]) * (self.fXj[:3, :] * a_partial_da)
data.da0_dx[:, :] = np.hstack([da0_dq, da0_dv])
def calcDiff(self, data, x, u, recalc=True):
if recalc:
self.calc(data, x, u)
pinocchio.updateFramePlacements(self.state.pinocchio, data.pinocchio)
data.rJf = pinocchio.Jlog6(data.rMf)
data.fJf = pinocchio.getFrameJacobian(self.state.pinocchio,
data.pinocchio, self.Mref.frame,
pinocchio.ReferenceFrame.LOCAL)
data.J = data.rJf * data.fJf
self.activation.calcDiff(data.activation, data.r, recalc)
data.Rx = np.hstack([
data.J,
pinocchio.utils.zero((self.activation.nr, self.state.nv))
])
data.Lx = np.vstack([
data.J.T * data.activation.Ar,
pinocchio.utils.zero((self.state.nv, 1))
])
data.Lxx = np.vstack([
np.hstack([
data.J.T * data.activation.Arr * data.J,
pinocchio.utils.zero((self.state.nv, self.state.nv))
]),
pinocchio.utils.zero((self.state.nv, self.state.ndx))
])
def Jtf(q, f):
pinocchio.computeJointJacobians(rmodel, rdata, q)
pinocchio.forwardKinematics(rmodel, rdata, q)
pinocchio.updateFramePlacements(rmodel, rdata)
J = pinocchio.getFrameJacobian(rmodel, rdata, CONTACTFRAME,
pinocchio.ReferenceFrame.LOCAL)
return J.T * f
def _inverse_kinematics_step(
self, frame_id: int, xdes: np.ndarray,
q0: np.ndarray) -> typing.Tuple[np.ndarray, np.ndarray]:
"""Compute one IK iteration for a single finger."""
dt = 1.0e-1
pinocchio.computeJointJacobians(
self.robot_model,
self.data,
q0,
)
pinocchio.framesForwardKinematics(
self.robot_model,
self.data,
q0,
)
Ji = pinocchio.getFrameJacobian(
self.robot_model,
self.data,
frame_id,
pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED,
)[:3, :]
xcurrent = self.data.oMf[frame_id].translation
try:
Jinv = np.linalg.inv(Ji)
except Exception:
Jinv = np.linalg.pinv(Ji)
err = xdes - xcurrent
dq = Jinv.dot(xdes - xcurrent)
qnext = pinocchio.integrate(self.robot_model, q0, dt * dq)
return qnext, err
def calcDiff(self, q):
pin.framesForwardKinematics(self.rmodel, self.rdata, q)
pin.computeJointJacobians(self.rmodel, self.rdata, q)
M = self.rdata.oMf[self.frameIndex]
J = pin.getFrameJacobian(self.rmodel, self.rdata, self.frameIndex,
pin.LOCAL_WORLD_ALIGNED)[:3, :]
return 2 * J.T @ (M.translation - self.ptarget)
def get_world_oriented_frame_jacobian(self, index):
self.robot.forwardKinematics(self.q, self.dq)
self.robot.computeJointJacobians(self.q)
self.robot.framesForwardKinematics(self.q)
jac = pinocchio.getFrameJacobian(self.model, self.data, index,
pinocchio.ReferenceFrame.LOCAL)
world_R_joint = pinocchio.SE3(self.data.oMf[index].rotation, zero(3))
return world_R_joint.action.dot(jac)
def MvJtf(q, vnext, v, f):
M = pinocchio.crba(rmodel, rdata, q)
pinocchio.computeJointJacobians(rmodel, rdata, q)
pinocchio.forwardKinematics(rmodel, rdata, q)
pinocchio.updateFramePlacements(rmodel, rdata)
J = pinocchio.getFrameJacobian(rmodel, rdata, CONTACTFRAME,
pinocchio.ReferenceFrame.LOCAL)
return M * (vnext - v) - J.T * f
def computeJacobian(self, q):
pin.forwardKinematics(self.rmodel, self.rdata, q)
pin.updateFramePlacements(self.rmodel, self.rdata)
pin.computeJointJacobians(self.rmodel, self.rdata, q)
J = pin.getFrameJacobian(self.rmodel, self.rdata, self.ee_frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
#modify J to remove the term corresponding to the base frame orientation
J = np.hstack([J[:, :3], np.zeros((6, 4)), J[:, 6:]])
return J
def main():
'''
Main procedure
1 ) Build the model from an URDF file
2 ) compute forwardKinematics
3 ) compute forwardKinematics of the frames
4 ) explore data
'''
model_file = Path(__file__).absolute().parents[1].joinpath(
'urdf', 'twolinks.urdf')
model = buildModelFromUrdf(str(model_file))
# Create data required by the algorithms
data = model.createData()
# Sample a random configuration
numpy_vector_joint_pos = randomConfiguration(model)
numpy_vector_joint_vel = np.random.rand(model.njoints - 1)
# Perform the forward kinematics over the kinematic tree
forwardKinematics(model, data, numpy_vector_joint_pos,
numpy_vector_joint_vel)
computeJointJacobians(model, data, numpy_vector_joint_pos)
joint_number = model.njoints
for i in range(joint_number):
joint = model.joints[i]
joint_placement = data.oMi[i]
joint_velocity = getVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
joint_jacobian = getJointJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
joint_linear_vel = joint_velocity.linear
joint_angular_vel = joint_velocity.angular
joint_6v_vel = joint_velocity.vector
err = joint_6v_vel - joint_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
# CAUTION updateFramePlacements must be called to update frame's positions
# Remark: in pinocchio frames, joints and bodies are different things
updateFramePlacements(model, data)
frame_number = model.nframes
for i in range(frame_number):
frame = model.frames[i]
frame_placement = data.oMf[i]
frame_velocity = getFrameVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
frame_jacobian = getFrameJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
frame_linear_vel = frame_velocity.linear
frame_angular_vel = frame_velocity.angular
frame_6v_vel = frame_velocity.vector
err = frame_6v_vel - frame_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
def calcDiff(self, q, recalc=False):
if recalc:
self.calc(q)
J = np.dot(
pin.Jlog3(self.rMf),
pin.getFrameJacobian(self.rmodel, self.rdata, self.ee_frame_id,
pin.ReferenceFrame.LOCAL)[3:, :])
self.J = self.weight_matrix.dot(J)
return self.J
def dConstraint_dx(self, x):
q = a2m(x)
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.computeJointJacobians(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
refMeff = self.refEff.inverse() * self.rdata.oMf[self.idEff]
log_M = pinocchio.Jlog6(refMeff)
M_q = pinocchio.getFrameJacobian(self.rmodel, self.rdata, self.idEff,
LOCAL)
self.Jeq = log_M * M_q
return self.Jeq
def calc(self, data, x):
assert (self.Mref.oMf is not None or self.gains[0] == 0.)
data.Jc = pinocchio.getFrameJacobian(self.state.pinocchio, data.pinocchio, self.Mref.frame,
pinocchio.ReferenceFrame.LOCAL)
data.a0 = pinocchio.getFrameAcceleration(self.state.pinocchio, data.pinocchio, self.Mref.frame).vector
if self.gains[0] != 0.:
self.rMf = self.Mref.oMf.inverse() * data.pinocchio.oMf[self.Mref.frame]
data.a0 += np.asscalar(self.gains[0]) * pinocchio.log6(self.rMf).vector
if self.gains[1] != 0.:
v = pinocchio.getFrameVelocity(self.state.pinocchio, data.pinocchio, self.Mref.frame).vector
data.a0 += np.asscalar(self.gains[1]) * v
def get_link_jacobian(self, link_id):
frame_id = self._model.getFrameId(link_id)
pin.computeJointJacobians(self._model, self._data, self._q)
jac = pin.getFrameJacobian(self._model, self._data, frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
# Pinocchio has linear on top of angular
ret = np.zeros_like(jac)
ret[0:3] = jac[3:6]
ret[3:6] = jac[0:3]
return np.copy(ret)
def getJacobian(self, q=None):
if q is None:
q = self.current_j_pos
if q.shape[0] == 7:
q_tmp = q.copy()
q = np.zeros(9)
q[:7] = q_tmp
pinocchio.computeJointJacobians(self.model, self.data, q)
pinocchio.framesForwardKinematics(self.model, self.data, q)
return pinocchio.getFrameJacobian(self.model, self.data,
self.end_effector_frame_id,
pinocchio.LOCAL_WORLD_ALIGNED)[:, :7]
def calcDiff(self, q, recalc=False):
if recalc:
self.calc(q)
R = self.rdata.oMf[self.ee_frame_id].rotation
J = R.dot(
pin.getFrameJacobian(self.rmodel, self.rdata, self.ee_frame_id,
pin.ReferenceFrame.LOCAL)[:3, :])
self.J = self.weight_matrix.dot(J)
return self.J
def main():
'''
Main procedure
1 ) Build the model from an URDF file
2 ) compute forwardKinematics
3 ) compute forwardKinematics of the frames
4 ) explore data
'''
model_file = Path(__file__).absolute().parents[1].joinpath(
'urdf', 'twolinks.urdf')
model = buildModelFromUrdf(str(model_file))
# Create data required by the algorithms
data = model.createData()
# Sample a random configuration
numpy_vector_joint_pos = randomConfiguration(model)
numpy_vector_joint_vel = np.random.rand(model.njoints - 1)
numpy_vector_joint_acc = np.random.rand(model.njoints - 1)
# Calls Reverese Newton-Euler algorithm
numpy_vector_joint_torques = rnea(model, data, numpy_vector_joint_pos,
numpy_vector_joint_vel,
numpy_vector_joint_acc)
# IN WHAT FOLLOWS WE CONFIRM THAT rnea COMPUTES THE FORWARD KINEMATCS
computeJointJacobians(model, data, numpy_vector_joint_pos)
joint_number = model.njoints
for i in range(joint_number):
joint = model.joints[i]
joint_placement = data.oMi[i]
joint_velocity = getVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
joint_jacobian = getJointJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
err = joint_velocity.vector - \
joint_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
# CAUTION updateFramePlacements must be called to update frame's positions
# Remark: in pinocchio frames, joints and bodies are different things
updateFramePlacements(model, data)
frame_number = model.nframes
for i in range(frame_number):
frame = model.frames[i]
frame_placement = data.oMf[i]
frame_velocity = getFrameVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
frame_jacobian = getFrameJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
err = frame_velocity.vector - \
frame_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
def calcDiff(self, data, x, u):
pinocchio.updateFramePlacements(self.state.pinocchio, data.shared.pinocchio)
data.R = data.shared.pinocchio.oMf[self.xref.frame].rotation
data.J = data.R * pinocchio.getFrameJacobian(self.state.pinocchio, data.shared.pinocchio, self.xref.frame,
pinocchio.ReferenceFrame.LOCAL)[:3, :]
self.activation.calcDiff(data.activation, data.r)
data.Rx = np.hstack([data.J, pinocchio.utils.zero((self.activation.nr, self.state.nv))])
data.Lx = np.vstack([data.J.T * data.activation.Ar, pinocchio.utils.zero((self.state.nv, 1))])
data.Lxx = np.vstack([
np.hstack([data.J.T * data.activation.Arr * data.J,
pinocchio.utils.zero((self.state.nv, self.state.nv))]),
pinocchio.utils.zero((self.state.nv, self.state.ndx))
])
def dConstraint_dx_rightfoot(self, x, nc=0):
q = self.vq2q(a2m(x))
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
pinocchio.computeJointJacobians(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
refMr = self.refR.inverse() * self.rdata.oMf[self.idR]
log_M = pinocchio.Jlog6(refMr)
M_q = pinocchio.getFrameJacobian(self.rmodel, self.rdata, self.idR,
LOCAL)
Q_v = self.dq_dv(a2m(x))
self.Jeq[nc:nc + 6, :] = log_M * M_q * Q_v
return self.Jeq[nc:nc + 6, :]
def calcDiff(self, data, x, u):
pinocchio.updateFramePlacements(self.state.pinocchio, data.shared.pinocchio)
data.rJf = pinocchio.Jlog3(data.rRf)
data.fJf = pinocchio.getFrameJacobian(self.state.pinocchio, data.shared.pinocchio, self.Rref.frame,
pinocchio.ReferenceFrame.LOCAL)[3:, :]
data.J = data.rJf * data.fJf
self.activation.calcDiff(data.activation, data.r)
data.Rx = np.hstack([data.J, pinocchio.utils.zero((self.activation.nr, self.state.nv))])
data.Lx = np.vstack([data.J.T * data.activation.Ar, pinocchio.utils.zero((self.state.nv, 1))])
data.Lxx = np.vstack([
np.hstack([data.J.T * data.activation.Arr * data.J,
pinocchio.utils.zero((self.state.nv, self.state.nv))]),
pinocchio.utils.zero((self.state.nv, self.state.ndx))
])
def Jacobian(self, q):
"""
Description:
1. Computes the Jacobian Tensor of the robot.
Args:
q -> np.array (3,) : Joint Positions of the robot.
Returns:
np.array (3,3): Jacobian Tensor of the robot.
"""
frame = pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
pin.computeJointJacobians(self.model, self.model_data, q)
return pin.getFrameJacobian(self.model, self.model_data, self.EOAT_ID,
frame)[:3]
def calc(self, data, x):
# We suppose forwardKinematics(q,v,a), computeJointJacobian and updateFramePlacement already
# computed.
assert (self.ref is not None or self.gains[0] == 0.)
data.J[:, :] = pinocchio.getFrameJacobian(
self.pinocchio, data.pinocchio, self.frame,
pinocchio.ReferenceFrame.LOCAL)
data.a0[:] = pinocchio.getFrameAcceleration(self.pinocchio,
data.pinocchio,
self.frame).vector.flat
if self.gains[0] != 0.:
data.rMf = self.ref.inverse() * data.pinocchio.oMf[self.frame]
data.a0[:] += self.gains[0] * m2a(pinocchio.log(data.rMf).vector)
if self.gains[1] != 0.:
data.a0[:] += self.gains[1] * m2a(
pinocchio.getFrameVelocity(self.pinocchio, data.pinocchio,
self.frame).vector)
def calcDiff(self, data, x, u, recalc=True):
if recalc:
self.calc(data, x, u)
nq = self.nq
pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)
R = data.pinocchio.oMf[self.frame].rotation
J = R * pinocchio.getFrameJacobian(
self.pinocchio, data.pinocchio, self.frame,
pinocchio.ReferenceFrame.LOCAL)[:3, :]
Ax, Axx = self.activation.calcDiff(data.activation,
data.residuals,
recalc=recalc)
data.Rq[:, :nq] = J
data.Lq[:] = np.dot(J.T, Ax)
data.Lqq[:, :] = np.dot(data.Rq.T, Axx *
data.Rq) # J is a matrix, use Rq instead.
return data.cost
def calc(self, data, x):
assert (self.xref.oxf is not None or self.gains[0] == 0.)
v = pinocchio.getFrameVelocity(self.state.pinocchio, data.pinocchio, self.xref.frame)
self.vw = v.angular
self.vv = v.linear
fJf = pinocchio.getFrameJacobian(self.state.pinocchio, data.pinocchio, self.xref.frame,
pinocchio.ReferenceFrame.LOCAL)
data.Jc = fJf[:3, :]
self.Jw = fJf[3:, :]
data.a0 = pinocchio.getFrameAcceleration(self.state.pinocchio, data.pinocchio,
self.xref.frame).linear + pinocchio.utils.cross(self.vw, self.vv)
if self.gains[0] != 0.:
data.a0 += np.asscalar(self.gains[0]) * (data.pinocchio.oMf[self.xref.frame].translation - self.xref.oxf)
if self.gains[1] != 0.:
data.a0 += np.asscalar(self.gains[1]) * self.vv
def check_offset_to_pinocchio_model_in_cart_space(self, q):
import pinocchio
from classic_framework.utils.geometric_transformation import mat2quat
obj_urdf = sim_framework_path("./envs/panda_arm_hand_pinocchio.urdf")
model = pinocchio.buildModelFromUrdf(obj_urdf)
data = model.createData()
q_pin = np.zeros(9)
q_pin[:7] = q
pinocchio.framesForwardKinematics(model, data, q_pin)
pinocchio.computeJointJacobians(model, data, q_pin)
pinocchio.framesForwardKinematics(model, data, q_pin)
# get orientation
rotation_matrix = data.oMf[model.getFrameId('panda_grasptarget')].rotation
quaternions = mat2quat(rotation_matrix) # [w x y z]
jac = pinocchio.getFrameJacobian(model, data, model.getFrameId('panda_grasptarget'),
pinocchio.LOCAL_WORLD_ALIGNED)[:, :7]
print('position offset:',
self.current_c_pos - np.array(data.oMf[model.getFrameId('panda_grasptarget')].translation))
