def getFrameTrajectoryFromSolver(self, solver):
if len(self.frameTrajNames) == 0:
return None
ps = {fr: [] for fr in self.frameTrajNames}
models = solver.problem.runningModels.tolist() + [
solver.problem.terminalModel
]
datas = solver.problem.runningDatas.tolist() + [
solver.problem.terminalData
]
for key, p in ps.items():
frameId = int(key)
for i, data in enumerate(datas):
model = models[i]
if hasattr(data, "differential"):
# Update the frame placement if there is not contact.
# Note that, in non-contact cases, the action model does not compute it for efficiency reason
if len(data.differential.multibody.contacts.contacts.
todict().items()) == 0:
pinocchio.updateFramePlacement(
model.differential.state.pinocchio,
data.differential.multibody.pinocchio, frameId)
pose = data.differential.multibody.pinocchio.oMf[frameId]
p.append(
np.asarray(pose.translation.T).reshape(-1).tolist())
elif isinstance(
data,
libcrocoddyl_pywrap.ActionDataImpulseFwdDynamics):
pose = data.multibody.pinocchio.oMf[frameId]
p.append(
np.asarray(pose.translation.T).reshape(-1).tolist())
return ps
def get_link_iso(self, link_id):
ret = np.eye(4)
frame_id = self._model.getFrameId(link_id)
trans = pin.updateFramePlacement(self._model, self._data, frame_id)
ret[0:3, 0:3] = trans.rotation
ret[0:3, 3] = trans.translation
return np.copy(ret)
def BaseVelocityFromKinAndIMU(self, contactFrameId):
"""Estimate the velocity of the base with forward kinematics using a contact point
that is supposed immobile in world frame
Args:
contactFrameId (int): ID of the contact point frame (foot frame)
"""
frameVelocity = pin.getFrameVelocity(self.model, self.data,
contactFrameId,
pin.ReferenceFrame.LOCAL)
framePlacement = pin.updateFramePlacement(self.model, self.data,
contactFrameId)
# Angular velocity of the base wrt the world in the base frame (Gyroscope)
_1w01 = self.IMU_ang_vel.reshape((3, 1))
# Linear velocity of the foot wrt the base in the foot frame
_Fv1F = frameVelocity.linear
# Level arm between the base and the foot
_1F = np.array(framePlacement.translation)
# Orientation of the foot wrt the base
_1RF = framePlacement.rotation
# Linear velocity of the base wrt world in the base frame
_1v01 = self.cross3(_1F.ravel(), _1w01.ravel()) - \
(_1RF @ _Fv1F.reshape((3, 1)))
# IMU and base frames have the same orientation
# _iv0i = _1v01 + self.cross3(self._1Mi.translation.ravel(), _1w01.ravel())
return np.array(_1v01)
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 callback(self, q):
print(1)
q = np.matrix(q).T
pin.forwardKinematics(robot.model, robot.data, q)
M = pin.updateFramePlacement(robot.model, robot.data, self.frameIndex)
gv.applyConfiguration('world/blue', pin.SE3ToXYZQUATtuple(M))
gv.applyConfiguration('world/box', pin.SE3ToXYZQUATtuple(self.Mtarget))
robot.display(q)
time.sleep(1e-2)
def test_getters(self):
data = self.model.createData()
q = pin.randomConfiguration(self.model)
v = np.random.rand(self.model.nv)
a = np.random.rand(self.model.nv)
pin.forwardKinematics(self.model, data, q, v, a)
T = pin.updateFramePlacement(self.model, data, self.frame_idx)
self.assertApprox(T,
data.oMi[self.parent_idx].act(self.frame_placement))
v = pin.getFrameVelocity(self.model, data, self.frame_idx)
self.assertApprox(v,
self.frame_placement.actInv(data.v[self.parent_idx]))
v = pin.getFrameVelocity(self.model, data, self.frame_idx,
pin.ReferenceFrame.LOCAL)
self.assertApprox(v,
self.frame_placement.actInv(data.v[self.parent_idx]))
v = pin.getFrameVelocity(self.model, data, self.frame_idx,
pin.ReferenceFrame.WORLD)
self.assertApprox(
v, data.oMi[self.parent_idx].act(data.v[self.parent_idx]))
v = pin.getFrameVelocity(self.model, data, self.frame_idx,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
self.assertApprox(
v,
pin.SE3(T.rotation, np.zeros(3)).act(
self.frame_placement.actInv(data.v[self.parent_idx])))
a = pin.getFrameAcceleration(self.model, data, self.frame_idx)
self.assertApprox(a,
self.frame_placement.actInv(data.a[self.parent_idx]))
a = pin.getFrameAcceleration(self.model, data, self.frame_idx,
pin.ReferenceFrame.LOCAL)
self.assertApprox(a,
self.frame_placement.actInv(data.a[self.parent_idx]))
a = pin.getFrameAcceleration(self.model, data, self.frame_idx,
pin.ReferenceFrame.WORLD)
self.assertApprox(
a, data.oMi[self.parent_idx].act(data.a[self.parent_idx]))
a = pin.getFrameAcceleration(self.model, data, self.frame_idx,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
self.assertApprox(
a,
pin.SE3(T.rotation, np.zeros(3)).act(
self.frame_placement.actInv(data.a[self.parent_idx])))
a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx)
a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx,
pin.ReferenceFrame.LOCAL)
a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx,
pin.ReferenceFrame.WORLD)
a = pin.getFrameClassicalAcceleration(
self.model, data, self.frame_idx,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
def createEESplines(self, rmodel, rdata, xs, t_sampling=0.005):
N = len(xs)
abscissa = a2m(np.linspace(0., t_sampling * (N - 1), N))
self.ee_splines = EESplines()
for patch in self.ee_map.keys():
p = np.zeros((3, N))
m = np.zeros((3, N))
for i in range(N):
q = a2m(xs[i][:rmodel.nq])
v = a2m(xs[i][-rmodel.nv:])
pinocchio.forwardKinematics(rmodel, rdata, q, v)
p[:, i] = m2a(
pinocchio.updateFramePlacement(rmodel, rdata, rmodel.getFrameId(self.ee_map[patch])).translation)
m[:, i] = m2a(pinocchio.getFrameVelocity(rmodel, rdata, rmodel.getFrameId(self.ee_map[patch])).linear)
self.ee_splines.update([[patch, CubicHermiteSpline(abscissa, p, m)]])
return
def ForwardKinematics(self, q, v=np.zeros(3)):
"""
Description:
1. Computes the Cartesian Position & Velocity of the robot
2. The TCP position is w.r.t. the 'tip_link' of the robot.
Args:
q -> np.array (3,) : Joint Positions of the robot.
v -> np.array (3,) : Joint Velocities of the robot.
Returns:
np.array (3,) : Cartesian Position of the robot.
np.array (3,) : Cartesian Velocity of the robot.
"""
index = self.EOAT_ID
frame = pin.ReferenceFrame.LOCAL
pin.forwardKinematics(self.model, self.model_data, q, v)
pin.updateFramePlacements(self.model, self.model_data)
pos = pin.updateFramePlacement(self.model, self.model_data, index)
vel = pin.getFrameVelocity(self.model, self.model_data, index, frame)
return np.array(pos)[:3, -1], np.array(vel)[:3]
def writeToMessage(self,
t,
q,
v=None,
tau=None,
p=dict(),
pd=dict(),
f=dict(),
s=dict()):
# Filling the time information
self._msg.header.stamp = rospy.Time(t)
self._msg.time = t
if v is None:
v = np.zeros(self._model.nv)
if tau is None:
tau = np.zeros(self._model.njoints - 2)
# Filling the centroidal state
pinocchio.centerOfMass(self._model, self._data, q, v)
c = self._data.com[0]
cd = self._data.vcom[0]
# Center of mass
self._msg.centroidal.com_position.x = c[0]
self._msg.centroidal.com_position.y = c[1]
self._msg.centroidal.com_position.z = c[2]
self._msg.centroidal.com_velocity.x = cd[0]
self._msg.centroidal.com_velocity.y = cd[1]
self._msg.centroidal.com_velocity.z = cd[2]
# Base
self._msg.centroidal.base_orientation.x = q[3]
self._msg.centroidal.base_orientation.y = q[4]
self._msg.centroidal.base_orientation.z = q[5]
self._msg.centroidal.base_orientation.w = q[6]
self._msg.centroidal.base_angular_velocity.x = v[3]
self._msg.centroidal.base_angular_velocity.y = v[4]
self._msg.centroidal.base_angular_velocity.z = v[5]
# Momenta
momenta = pinocchio.computeCentroidalMomentum(self._model, self._data)
momenta_rate = pinocchio.computeCentroidalMomentumTimeVariation(
self._model, self._data)
self._msg.centroidal.momenta.linear.x = momenta.linear[0]
self._msg.centroidal.momenta.linear.y = momenta.linear[1]
self._msg.centroidal.momenta.linear.z = momenta.linear[2]
self._msg.centroidal.momenta.angular.x = momenta.angular[0]
self._msg.centroidal.momenta.angular.y = momenta.angular[1]
self._msg.centroidal.momenta.angular.z = momenta.angular[2]
self._msg.centroidal.momenta_rate.linear.x = momenta_rate.linear[0]
self._msg.centroidal.momenta_rate.linear.y = momenta_rate.linear[1]
self._msg.centroidal.momenta_rate.linear.z = momenta_rate.linear[2]
self._msg.centroidal.momenta_rate.angular.x = momenta_rate.angular[0]
self._msg.centroidal.momenta_rate.angular.y = momenta_rate.angular[1]
self._msg.centroidal.momenta_rate.angular.z = momenta_rate.angular[2]
# Filling the joint state
njoints = self._model.njoints - 2
for j in range(njoints):
self._msg.joints[j].position = q[7 + j]
self._msg.joints[j].velocity = v[6 + j]
self._msg.joints[j].effort = tau[j]
# Filling the contact state
names = list(p.keys()) + list(pd.keys()) + list(f.keys()) + list(
s.keys())
names = list(dict.fromkeys(names))
self._msg.contacts = [None] * len(names)
if len(names) != 0 and (len(p.keys()) == 0 or len(pd.keys()) == 0):
pinocchio.forwardKinematics(self._model, self._data, q, v)
for i, name in enumerate(names):
contact_msg = ContactState()
contact_msg.name = name
frame_id = self._model.getFrameId(name)
# Retrive the contact position
if name in p:
pose = pinocchio.SE3ToXYZQUAT(p[name])
else:
oMf = pinocchio.updateFramePlacement(self._model, self._data,
frame_id)
pose = pinocchio.SE3ToXYZQUAT(oMf)
# Retrieve the contact velocity
if name in pd:
ovf = pd[name]
else:
ovf = pinocchio.getFrameVelocity(
self._model, self._data, frame_id,
pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED)
# Storing the contact position and velocity inside the message
contact_msg.pose.position.x = pose[0]
contact_msg.pose.position.y = pose[1]
contact_msg.pose.position.z = pose[2]
contact_msg.pose.orientation.x = pose[3]
contact_msg.pose.orientation.y = pose[4]
contact_msg.pose.orientation.z = pose[5]
contact_msg.pose.orientation.w = pose[6]
contact_msg.velocity.linear.x = ovf.linear[0]
contact_msg.velocity.linear.y = ovf.linear[1]
contact_msg.velocity.linear.z = ovf.linear[2]
contact_msg.velocity.angular.x = ovf.angular[0]
contact_msg.velocity.angular.y = ovf.angular[1]
contact_msg.velocity.angular.z = ovf.angular[2]
# Retrieving and storing force data
if name in f:
contact_info = f[name]
ctype = contact_info[0]
force = contact_info[1]
contact_msg.type = ctype
contact_msg.wrench.force.x = force.linear[0]
contact_msg.wrench.force.y = force.linear[1]
contact_msg.wrench.force.z = force.linear[2]
contact_msg.wrench.torque.x = force.angular[0]
contact_msg.wrench.torque.y = force.angular[1]
contact_msg.wrench.torque.z = force.angular[2]
if name in s:
terrain_info = s[name]
norm = terrain_info[0]
friction = terrain_info[1]
contact_msg.surface_normal.x = norm[0]
contact_msg.surface_normal.y = norm[1]
contact_msg.surface_normal.z = norm[2]
contact_msg.friction_coefficient = friction
self._msg.contacts[i] = contact_msg
return copy.deepcopy(self._msg)
def residual(self, q):
'''Compute score from a configuration'''
pin.forwardKinematics(self.rmodel, self.rdata, q)
M = pin.updateFramePlacement(self.rmodel, self.rdata, self.frameIndex)
self.deltaM = self.Mtarget.inverse() * M
return pin.log(self.deltaM).vector
def update_frame(robot, data, name):
frame_id = robot.getFrameId(name)
pin.updateFramePlacement(robot, data, frame_id)
def update_frame(model, data, name):
frame_id = model.getFrameId(name)
pnc.updateFramePlacement(model, data, frame_id)
def run_filter(self, k, gait, device, goals):
"""Run the complementary filter to get the filtered quantities
Args:
k (int): Number of inv dynamics iterations since the start of the simulation
gait (4xN array): Contact state of feet (gait matrix)
device (object): Interface with the masterboard or the simulation
goals (3x4 array): Target locations of feet on the ground
"""
feet_status = gait[0, :].copy() # Current contact state of feet
remaining_steps = 1 # Remaining MPC steps for the current gait phase
while (np.array_equal(feet_status, gait[remaining_steps, :])):
remaining_steps += 1
# Update IMU data
self.get_data_IMU(device)
# Angular position of the trunk
self.filt_ang_pos[:] = self.IMU_ang_pos
# Angular velocity of the trunk
self.filt_ang_vel[:] = self.IMU_ang_vel
# Update joints data
self.get_data_joints(device)
# Update nb of iterations since contact
self.k_since_contact += feet_status # Increment feet in stance phase
self.k_since_contact *= feet_status # Reset feet in swing phase
# Update forward kinematics data
self.get_data_FK(feet_status)
# Update forward geometry data
self.get_xyz_feet(feet_status, goals)
# Tune alpha depending on the state of the gait (close to contact switch or not)
a = np.ceil(np.max(self.k_since_contact) / 10) - 1
b = remaining_steps
n = 1 # Nb of steps of margin around contact switch
v_max = 1.00
v_min = 0.97 # Minimum alpha value
c = ((a + b) - 2 * n) * 0.5
if (a <= (n - 1)) or (b <= n): # If we are close from contact switch
self.alpha = v_max # Only trust IMU data
self.close_from_contact = True # Raise flag
else:
self.alpha = v_min + (v_max - v_min) * np.abs(c - (a - n)) / c
#self.alpha = 0.997
self.close_from_contact = False # Lower flag
if not self.kf_enabled: # Use cascade of complementary filters
# Rotation matrix to go from base frame to world frame
oRb = pin.Quaternion(np.array([self.IMU_ang_pos
]).T).toRotationMatrix()
"""self.debug_o_lin_vel += 0.002 * (oRb @ np.array([self.IMU_lin_acc]).T) # TOREMOVE
self.filt_lin_vel[:] = (oRb.T @ self.debug_o_lin_vel).ravel()"""
# Get FK estimated velocity at IMU location (base frame)
cross_product = self.cross3(self._1Mi.translation.ravel(),
self.IMU_ang_vel).ravel()
i_FK_lin_vel = self.FK_lin_vel[:] + cross_product
# Get FK estimated velocity at IMU location (world frame)
oi_FK_lin_vel = (oRb @ np.array([i_FK_lin_vel]).T).ravel()
# Integration of IMU acc at IMU location (world frame)
oi_filt_lin_vel = self.filter_xyz_vel.compute(
oi_FK_lin_vel, (oRb @ np.array([self.IMU_lin_acc]).T).ravel(),
alpha=self.alpha)
# Filtered estimated velocity at IMU location (base frame)
i_filt_lin_vel = (oRb.T @ np.array([oi_filt_lin_vel]).T).ravel()
# Filtered estimated velocity at center base (base frame)
b_filt_lin_vel = i_filt_lin_vel - cross_product
# Filtered estimated velocity at center base (world frame)
ob_filt_lin_vel = (oRb @ np.array([b_filt_lin_vel]).T).ravel()
# Position of the center of the base from FGeometry and filtered velocity (world frame)
self.filt_lin_pos[:] = self.filter_xyz_pos.compute(
self.FK_xyz[:] + self.xyz_mean_feet[:],
ob_filt_lin_vel,
alpha=np.array([0.995, 0.995, 0.9]))
# Velocity of the center of the base (base frame)
self.filt_lin_vel[:] = b_filt_lin_vel
else: # Use Kalman filter
# Rotation matrix to go from base frame to world frame
oRb = pin.Quaternion(np.array([self.IMU_ang_pos
]).T).toRotationMatrix()
# Update coefficients depending on feet status
self.kf.updateCoeffs(feet_status)
# Prediction step of the Kalman filter with IMU acceleration
self.kf.predict(oRb @ self.IMU_lin_acc.reshape((3, 1)))
# Get position of IMU relative to feet in base frame
for i in range(4):
framePlacement = -pin.updateFramePlacement(
self.model, self.data, self.indexes[i]).translation
self.Z[(3 * i):(3 * (i + 1)),
0:1] = oRb @ (framePlacement +
self._1Mi.translation.ravel()).reshape(
(3, 1))
self.Z[
12 + i,
0] = 0.0 # (oRb @ framePlacement.reshape((3, 1)))[2, 0] + self.filt_lin_pos[2]
# Correction step of the Kalman filter with position and velocity estimations by FK
# self.Z[0:3, 0] = self.FK_xyz[:] + self.xyz_mean_feet[:]
# self.Z[3:6, 0] = oRb.T @ self.FK_lin_vel
self.kf.correct(self.Z)
# Retrieve and store results
cross_product = self.cross3(self._1Mi.translation.ravel(),
self.IMU_ang_vel).ravel()
self.filt_lin_pos[:] = self.kf.X[0:3,
0] - self._1Mi.translation.ravel(
) # base position in world frame
self.filt_lin_vel[:] = oRb.transpose() @ (
self.kf.X[3:6, 0] - cross_product
) # base velocity in base frame
# Logging
self.log_alpha[self.k_log] = self.alpha
self.feet_status[:] = feet_status # Save contact status sent to the estimator for logging
self.feet_goals[:, :] = goals.copy(
) # Save feet goals sent to the estimator for logging
self.log_IMU_lin_acc[:, self.k_log] = self.IMU_lin_acc[:]
self.log_HP_lin_vel[:, self.k_log] = self.HP_lin_vel[:]
self.log_LP_lin_vel[:, self.k_log] = self.LP_lin_vel[:]
self.log_FK_lin_vel[:, self.k_log] = self.FK_lin_vel[:]
self.log_filt_lin_vel[:, self.k_log] = self.filt_lin_vel[:]
self.log_o_filt_lin_vel[:, self.k_log] = self.o_filt_lin_vel[:, 0]
# Output filtered position vector (19 x 1)
self.q_filt[0:3, 0] = self.filt_lin_pos
if self.perfectEstimator: # Base height directly from PyBullet
self.q_filt[2,
0] = device.dummyPos[2] - 0.0155 # Minus feet radius
self.q_filt[3:7, 0] = self.filt_ang_pos
self.q_filt[
7:,
0] = self.actuators_pos # Actuators pos are already directly from PyBullet
# Output filtered velocity vector (18 x 1)
if self.perfectEstimator: # Linear velocities directly from PyBullet
self.v_filt[0:3, 0] = (1 - self.alpha_v) * self.v_filt[
0:3, 0] + self.alpha_v * device.b_baseVel
else:
self.v_filt[0:3, 0] = (1 - self.alpha_v) * self.v_filt[
0:3, 0] + self.alpha_v * self.filt_lin_vel
self.v_filt[
3:6,
0] = self.filt_ang_vel # Angular velocities are already directly from PyBullet
self.v_filt[
6:,
0] = self.actuators_vel # Actuators velocities are already directly from PyBullet
###
# Update model used for the forward kinematics
"""pin.forwardKinematics(self.model, self.data, self.q_filt, self.v_filt)
pin.updateFramePlacements(self.model, self.data)
z_min = 100
for i in (np.where(feet_status == 1))[0]: # Consider only feet in contact
# Estimated position of the base using the considered foot
framePlacement = pin.updateFramePlacement(self.model, self.data, self.indexes[i])
z_min = np.min((framePlacement.translation[2], z_min))
self.q_filt[2, 0] -= z_min"""
###
# Output filtered actuators velocity for security checks
self.v_secu[:] = (
1 - self.alpha_secu
) * self.actuators_vel + self.alpha_secu * self.v_secu[:]
# Increment iteration counter
self.k_log += 1
return 0
def get_data_FK(self, feet_status):
"""Get data with forward kinematics and forward geometry
(linear velocity, angular velocity and position)
Args:
feet_status (4x0 numpy array): Current contact state of feet
"""
# Update estimator FK model
self.q_FK[7:, 0] = self.actuators_pos # Position of actuators
self.v_FK[6:, 0] = self.actuators_vel # Velocity of actuators
# Position and orientation of the base remain at 0
# Linear and angular velocities of the base remain at 0
# Update model used for the forward kinematics
self.q_FK[3:7, 0] = np.array([0.0, 0.0, 0.0, 1.0])
pin.forwardKinematics(self.model, self.data, self.q_FK, self.v_FK)
# pin.updateFramePlacements(self.model, self.data)
# Update model used for the forward geometry
self.q_FK[3:7, 0] = self.IMU_ang_pos[:]
pin.forwardKinematics(self.model_for_xyz, self.data_for_xyz, self.q_FK)
# Get estimated velocity from updated model
cpt = 0
vel_est = np.zeros((3, ))
xyz_est = np.zeros((3, ))
for i in (np.where(
feet_status == 1))[0]: # Consider only feet in contact
if self.k_since_contact[
i] >= 16: # Security margin after the contact switch
# Estimated velocity of the base using the considered foot
vel_estimated_baseframe = self.BaseVelocityFromKinAndIMU(
self.indexes[i])
# Estimated position of the base using the considered foot
framePlacement = pin.updateFramePlacement(
self.model_for_xyz, self.data_for_xyz, self.indexes[i])
xyz_estimated = -framePlacement.translation
# Logging
self.log_v_est[:, i, self.k_log] = vel_estimated_baseframe[0:3,
0]
self.log_h_est[i, self.k_log] = xyz_estimated[2]
# Increment counter and add estimated quantities to the storage variables
cpt += 1
vel_est += vel_estimated_baseframe[:, 0] # Linear velocity
xyz_est += xyz_estimated # Position
"""r_foot = 0.0155 # 31mm of diameter on meshlab
if i <= 1:
vel_est[0] += r_foot * (self.actuators_vel[1+3*i] - self.actuators_vel[2+3*i])
else:
vel_est[0] += r_foot * (self.actuators_vel[1+3*i] + self.actuators_vel[2+3*i])"""
# If at least one foot is in contact, we do the average of feet results
if cpt > 0:
self.FK_lin_vel = vel_est / cpt
self.FK_xyz = xyz_est / cpt
return 0
def residual3d(self, q):
'''Compute score from a configuration'''
pin.forwardKinematics(robot.model, robot.data, q)
M = pin.updateFramePlacement(robot.model, robot.data, self.frameIndex)
return (M.translation - self.Mtarget.translation)
model = pin.buildModelFromUrdf(urdf_file)
data = model.createData()
print(model)
q = np.array([np.pi / 2., 0., 0.])
# q = np.zeros(3)
qdot = np.ones(3)
pin.forwardKinematics(model, data, q, qdot)
## Print Frame Names
print([frame.name for frame in model.frames])
## Calculate j2 placement
j2_frame = model.getFrameId('j1')
j2_translation = pin.updateFramePlacement(model, data, j2_frame)
print("j2 translation")
print(j2_translation)
## Calculate l2 placement
l2_frame = model.getFrameId('l2')
l2_translation = pin.updateFramePlacement(model, data, l2_frame)
print("l2 translation")
print(l2_translation)
## Calculate j2 jacobian
pin.computeJointJacobians(model, data, q)
j2_jacobian = pin.getFrameJacobian(model, data, j2_frame,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
print("j2 jacobian")
print(j2_jacobian)
