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 *