def appendCentroidal(first_iter_for_phase = False): """ Compute the values of the CoM position, velocity, acceleration, the anuglar momentum and it's derivative from the wholebody data and append them to the current phase trajectories :param first_iter_for_phase: if True, set the initial values for the current phase and initialize the centroidal trajectories """ pcom, vcom, acom = pinRobot.com(q, v, dv) L = pinRobot.centroidalMomentum(q, v).angular dL = pin.computeCentroidalMomentumTimeVariation(pinRobot.model, pinRobot.data, q, v, dv).angular if first_iter_for_phase: phase.c_init = pcom phase.dc_init = vcom phase.ddc_init = acom phase.L_init = L phase.dL_init = dL phase.c_t = piecewise(polynomial(pcom.reshape(-1,1), t , t)) phase.dc_t = piecewise(polynomial(vcom.reshape(-1,1), t, t)) phase.ddc_t = piecewise(polynomial(acom.reshape(-1,1), t, t)) phase.L_t = piecewise(polynomial(L.reshape(-1,1), t, t)) phase.dL_t = piecewise(polynomial(dL.reshape(-1,1), t, t)) else: phase.c_t.append(pcom, t) phase.dc_t.append(vcom, t) phase.ddc_t.append(acom, t) phase.L_t.append(L, t) phase.dL_t.append(dL, t)
def test_centroidal_derivatives(self): res = pin.computeCentroidalDynamicsDerivatives(self.model,self.data,self.q,self.v,self.a) self.assertTrue(len(res) == 4) data2 = self.model.createData() pin.computeCentroidalMomentumTimeVariation(self.model,data2,self.q,self.v,self.a) self.assertApprox(self.data.hg,data2.hg) self.assertApprox(self.data.dhg,data2.dhg) data3 = self.model.createData() pin.computeRNEADerivatives(self.model,data3,self.q,self.v,self.a) res2 = pin.getCentroidalDynamicsDerivatives(self.model,data3) for k in range(4): self.assertApprox(res[k],res2[k])
def appendCentroidal(first_iter_for_phase=False): pcom, vcom, acom = pinRobot.com(q, v, dv) L = pinRobot.centroidalMomentum(q, v).angular dL = pin.computeCentroidalMomentumTimeVariation( pinRobot.model, pinRobot.data, q, v, dv).angular if first_iter_for_phase: phase.c_init = pcom phase.dc_init = vcom phase.ddc_init = acom phase.L_init = L phase.dL_init = dL phase.c_t = piecewise(polynomial(pcom.reshape(-1, 1), t, t)) phase.dc_t = piecewise(polynomial(vcom.reshape(-1, 1), t, t)) phase.ddc_t = piecewise(polynomial(acom.reshape(-1, 1), t, t)) phase.L_t = piecewise(polynomial(L.reshape(-1, 1), t, t)) phase.dL_t = piecewise(polynomial(dL.reshape(-1, 1), t, t)) else: phase.c_t.append(pcom, t) phase.dc_t.append(vcom, t) phase.ddc_t.append(acom, t) phase.L_t.append(L, t) phase.dL_t.append(dL, t)
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)
robot.computeAllTerms(data, q, v) sample = traj.computeNext() taskAM.setReference(sample) const = taskAM.compute(t, q, v, data) # compare the pinocchio method to # Del Prete's quick and dirty way to compute drift # compute momentum Jacobian at next time step assuming zero acc dt = 1e-3 q_next = pin.integrate(model, q, dt * v) data_next = robot.data().copy() robot.computeAllTerms(data_next, q_next, v) J_am = data.Ag J_am_next = data_next.Ag drift = (J_am_next[-3:, :] - J_am[-3:, :]) * v / dt drift_pin = pin.computeCentroidalMomentumTimeVariation(model, data).angular diff_drift = norm(drift_pin - drift) print("Difference between drift computations: ", diff_drift) Jpinv = np.linalg.pinv(const.matrix, 1e-5) dv = Jpinv * const.vector assert np.linalg.norm(Jpinv * const.matrix, 2) - 1.0 < tol v += dt * dv q = pin.integrate(model, q, dt * v) t += dt error = np.linalg.norm(taskAM.momentum_error, 2) assert error - error_past < 1e-4 error_past = error if error < 1e-8: