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: