def test_Jcom_noupdate2(self):
data_no = self.data
data_up = self.model.createData()
pin.forwardKinematics(self.model, data_no, self.q)
Jcom_no = pin.jacobianCenterOfMass(self.model, data_no)
Jcom_up = pin.jacobianCenterOfMass(self.model, data_up, self.q)
self.assertTrue((Jcom_no == Jcom_up).all())
def test_Jcom_noupdate2(self):
data_no = self.data
data_up = self.model.createData()
pin.forwardKinematics(self.model,data_no,self.q)
Jcom_no = pin.jacobianCenterOfMass(self.model,data_no)
Jcom_up = pin.jacobianCenterOfMass(self.model,data_up,self.q)
self.assertTrue((Jcom_no==Jcom_up).all())
def calcDiff(self, data, x, u=None, recalc=True):
if recalc:
self.calc(data, x, u)
self.assertImpactDiffDataSet(data)
nv = self.pinocchio.nv
Ax, Axx = self.activation.calcDiff(data.activation, data.residuals)
# TODO ???
# r = Jcom(vnext-v)
# dr/dv = Jcom*(dvnext/dv - I)
# dr/dq = dJcom_dq*(vnext-v) + Jcom*dvnext_dq
# = dvcom_dq(vq=vnext-v) + Jcom*dvnext_dq
# Jcom*v = M[:3,:]/mass * v = RNEA(q,0,v)[:3]/mass
# => dvcom/dq = dRNEA_dq(q,0,v)[:3,:]/mass
dvc_dq = pinocchio.getCenterOfMassVelocityDerivatives(
self.pinocchio, data.pinocchio_dv)
dvc_dv = pinocchio.jacobianCenterOfMass(self.pinocchio,
data.pinocchio_dv)
# res = vcom(q,vnext-v)
# dres/dq = dvcom_dq + dvcom_dv*dvnext_dq
data.Rx[:, :nv] = dvc_dq + np.dot(dvc_dv, data.dvnext_dx[:, :nv])
# dres/dv = dvcom_dv*(dvnext_dv-I)
ddv_dv = data.dvnext_dx[:, nv:].copy()
ddv_dv[range(nv), range(nv)] -= 1
data.Rx[:, nv:] = np.dot(dvc_dv, ddv_dv)
data.Lx[:] = np.dot(data.Rx.T, Ax)
data.Lxx[:, :] = np.dot(data.Rx.T, Axx * data.Rx)
def setUp(self):
self.robot_data = self.ROBOT_MODEL.createData()
self.x = self.ROBOT_STATE.rand()
self.u = pinocchio.utils.rand(self.ROBOT_MODEL.nv)
self.multibody_data = crocoddyl.DataCollectorMultibody(self.robot_data)
self.data = self.COST.createData(self.multibody_data)
self.data_der = self.COST_DER.createData(self.multibody_data)
nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv
pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:])
pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
pinocchio.utils.zero(nv))
pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data, self.x[:nq])
pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data)
pinocchio.jacobianCenterOfMass(self.ROBOT_MODEL, self.robot_data, self.x[:nq], False)
def calcDiff(self, q, recalc=False):
if recalc:
self.calc(q)
J = pin.jacobianCenterOfMass(self.rmodel, self.rdata, q)
#remove the gradient due to the base orientation
self.J = np.hstack([J[:, :3], np.zeros((3, 4)), J[:, 6:]])
self.Cx = self.J.T.dot(self.com - self.desired_pos)
return self.Cx
def calcDiff(self, q, recalc=False):
if recalc:
self.calc(q)
J = pin.jacobianCenterOfMass(self.rmodel, self.rdata, q)
stat = (self.com - self.margin < self.bounds[0]) + \
(self.com + self.margin > self.bounds[1])
self.J = (stat * self.identity).dot(J)
return self.J
def setUp(self):
self.robot_data = self.ROBOT_MODEL.createData()
self.x = self.ROBOT_STATE.rand()
self.u = pinocchio.utils.rand(self.ROBOT_MODEL.nv)
self.cost_sum = crocoddyl.CostModelSum(self.ROBOT_STATE)
self.cost_sum.addCost('myCost', self.COST, 1.)
self.data = self.COST.createData(self.robot_data)
self.data_sum = self.cost_sum.createData(self.robot_data)
nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv
pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data,
self.x[:nq], self.x[nq:])
pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL,
self.robot_data,
self.x[:nq], self.x[nq:],
pinocchio.utils.zero(nv))
pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data,
self.x[:nq])
pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data)
pinocchio.jacobianCenterOfMass(self.ROBOT_MODEL, self.robot_data,
self.x[:nq], False)
def test_subtree_jacobian(self):
model = self.model
data = self.data
Jcom = pin.jacobianCenterOfMass(model, data, self.q)
Jcom_subtree = pin.getJacobianSubtreeCenterOfMass(model, data, 0)
self.assertApprox(Jcom, Jcom_subtree)
data2 = model.createData()
Jcom_subtree2 = pin.jacobianSubtreeCenterOfMass(
model, data2, self.q, 0)
self.assertApprox(Jcom_subtree, Jcom_subtree2)
data3 = model.createData()
Jcom_subtree3 = pin.jacobianSubtreeCoMJacobian(model, data3, self.q, 0)
self.assertApprox(Jcom_subtree3, Jcom_subtree2)
Jcom_subtree4 = pin.jacobianSubtreeCoMJacobian(model, data3, 0)
self.assertApprox(Jcom_subtree3, Jcom_subtree4)
def playForwardKinematics(self, Q, sleep=0.0025, step=10, record=False):
''' playForwardKinematics(q, sleep, step, record)
'''
# TODO at verical line to plot as in opensim during playing
rec = {'q': [], 'com': [], 'Jcom': []}
for i in range(0, len(Q), step):
self.q = Q[i]
self.display(self.q,
osimref=True,
com=True,
updateKinematics=False)
time.sleep(sleep)
if record is True:
#rec = self.record()
rec['q'].append(self.q)
rec['com'].append(self.com(self.q).getA()[:, 0])
rec['Jcom'].append(
self.Jcom(
se3.jacobianCenterOfMass(self.model, self.data,
self.q)))
if record is True:
return rec
def Jcom(self, q): #, update_kinematics=True):
return se3.jacobianCenterOfMass(self.model, self.data, q)
def test_Jcom_update4(self):
Jcom = pin.jacobianCenterOfMass(self.model, self.data, self.q, True)
self.assertFalse(np.isnan(Jcom).any())
self.assertFalse(np.isnan(self.data.com[1]).any())
def test_Jcom_update3(self):
Jcom = pin.jacobianCenterOfMass(self.model, self.data, self.q)
self.assertFalse(np.isnan(Jcom).any())
import pinocchio as pio from example_robot_data import loadTalos np.set_printoptions(precision=1, threshold=1e4, suppress=True, linewidth=200) from numpy.linalg import norm, svd, eig, inv, pinv r = loadTalos() rm = r.model rd = rm.createData() q0 = r.q0.copy() q = pio.randomConfiguration(rm) vq = np.random.rand(rm.nv) * 2 - 1 b_Mc = pio.crba(rm, rd, q) # mass matrix at b frame expressed in b frame b_I_b = b_Mc[:6, :6] # inertia matrix at b frame expressed in b frame Jc = pio.jacobianCenterOfMass(rm, rd, q) w_T_b = rd.oMi[1] # world to body transform # Check that the Inertia matrix expressed at the center of mass is of the form: # c_I_c = b_Mc[:6,:6] = [[m*Id3 03 ] # [03 I(q)]] pio.centerOfMass(rm, rd, q, vq) c_T_b = pio.SE3(w_T_b.rotation, -rd.com[0] + w_T_b.translation) # Ic = cXb^star * Ib * bXc c_I_c = c_T_b.actionInverse.T @ b_I_b @ c_T_b.actionInverse # momentum coordinate transform assert (np.allclose(c_I_c[:3, :3], rd.mass[0] * np.eye(3))) # Check that M[:6,:] is the centroidal momentum expressed in F_b c_hc = pio.computeCentroidalMomentum(rm, rd, q, vq) b_hc = pio.Force(b_Mc[:6, :] @ vq)
def test_Jcom_update4(self):
Jcom = pin.jacobianCenterOfMass(self.model,self.data,self.q,True)
self.assertFalse(np.isnan(Jcom).any())
self.assertFalse(np.isnan(self.data.com[1]).any())
def test_Jcom_update3(self):
Jcom = pin.jacobianCenterOfMass(self.model,self.data,self.q)
self.assertFalse(np.isnan(Jcom).any())
def update_quantities(robot: jiminy.Model,
position: np.ndarray,
velocity: Optional[np.ndarray] = None,
acceleration: Optional[np.ndarray] = None,
update_physics: bool = True,
update_com: bool = True,
update_energy: bool = True,
update_jacobian: bool = False,
update_collisions: bool = True,
use_theoretical_model: bool = True) -> None:
"""Compute all quantities using position, velocity and acceleration
configurations.
Run multiple algorithms to compute all quantities which can be known with
the model position, velocity and acceleration.
This includes:
- body spatial transforms,
- body spatial velocities,
- body spatial drifts,
- body transform acceleration,
- body transform jacobians,
- center-of-mass position,
- center-of-mass velocity,
- center-of-mass drift,
- center-of-mass acceleration,
- center-of-mass jacobian,
- articular inertia matrix,
- non-linear effects (Coriolis + gravity)
- collisions and distances
.. note::
Computation results are stored internally in the robot, and can
be retrieved with associated getters.
.. warning::
This function modifies the internal robot data.
.. warning::
It does not called overloaded pinocchio methods provided by
`jiminy_py.core` but the original pinocchio methods instead. As a
result, it does not take into account the rotor inertias / armatures.
One is responsible of calling the appropriate methods manually instead
of this one if necessary. This behavior is expected to change in the
future.
:param robot: Jiminy robot.
:param position: Robot position vector.
:param velocity: Robot velocity vector.
:param acceleration: Robot acceleration vector.
:param update_physics: Whether or not to compute the non-linear effects and
internal/external forces.
Optional: True by default.
:param update_com: Whether or not to compute the COM of the robot AND each
link individually. The global COM is the first index.
Optional: False by default.
:param update_energy: Whether or not to compute the energy of the robot.
Optional: False by default
:param update_jacobian: Whether or not to compute the jacobians.
Optional: False by default.
:param use_theoretical_model: Whether the state corresponds to the
theoretical model when updating and fetching
the robot's state.
Optional: True by default.
"""
if use_theoretical_model:
pnc_model = robot.pinocchio_model_th
pnc_data = robot.pinocchio_data_th
else:
pnc_model = robot.pinocchio_model
pnc_data = robot.pinocchio_data
if (update_physics and update_com and
update_energy and update_jacobian and
velocity is not None and acceleration is None):
pin.computeAllTerms(pnc_model, pnc_data, position, velocity)
else:
if velocity is None:
pin.forwardKinematics(pnc_model, pnc_data, position)
elif acceleration is None:
pin.forwardKinematics(pnc_model, pnc_data, position, velocity)
else:
pin.forwardKinematics(
pnc_model, pnc_data, position, velocity, acceleration)
if update_com:
if velocity is None:
pin.centerOfMass(pnc_model, pnc_data, position)
elif acceleration is None:
pin.centerOfMass(pnc_model, pnc_data, position, velocity)
else:
pin.centerOfMass(
pnc_model, pnc_data, position, velocity, acceleration)
if update_jacobian:
if update_com:
pin.jacobianCenterOfMass(pnc_model, pnc_data)
pin.computeJointJacobians(pnc_model, pnc_data)
if update_physics:
if velocity is not None:
pin.nonLinearEffects(pnc_model, pnc_data, position, velocity)
pin.crba(pnc_model, pnc_data, position)
if update_energy:
if velocity is not None:
pin.computeKineticEnergy(pnc_model, pnc_data)
pin.computePotentialEnergy(pnc_model, pnc_data)
pin.updateFramePlacements(pnc_model, pnc_data)
if update_collisions:
pin.updateGeometryPlacements(
pnc_model, pnc_data, robot.collision_model, robot.collision_data)
pin.computeCollisions(
robot.collision_model, robot.collision_data,
stop_at_first_collision=False)
pin.computeDistances(robot.collision_model, robot.collision_data)
for dist_req in robot.collision_data.distanceResults:
if np.linalg.norm(dist_req.normal) < 1e-6:
pin.computeDistances(
robot.collision_model, robot.collision_data)
break
def get_com_lin_jacobian(self):
return np.copy(
pin.jacobianCenterOfMass(self._model, self._data, self._q))
costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(costData.Lxu, costDataND.Lxu, NUMDIFF_MODIFIER *
costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(costData.Luu, costDataND.Luu, NUMDIFF_MODIFIER *
costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
q = pinocchio.randomConfiguration(rmodel)
v = rand(rmodel.nv)
x = m2a(np.concatenate([q, v]))
u = m2a(rand(rmodel.nv))
costModel = CostModelCoM(rmodel, np.array([.5, .4, .3]))
costData = costModel.createData(rdata)
pinocchio.jacobianCenterOfMass(rmodel, rdata, q, False)
costModel.calcDiff(costData, x, u)
costModelND = CostModelNumDiff(
costModel,
StatePinocchio(rmodel),
withGaussApprox=True,
reevals=[lambda m, d, x, u: pinocchio.jacobianCenterOfMass(m, d, a2m(x[:rmodel.nq]), False)])
costDataND = costModelND.createData(rdata)
costModelND.calcDiff(costDataND, x, u)
assertNumDiff(costData.Lx, costDataND.Lx, NUMDIFF_MODIFIER *
costModelND.disturbance) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(costData.Lu, costDataND.Lu, NUMDIFF_MODIFIER *
