def dyn_value(self, t, q, v, update_geometry=False):
J_am = self.robot.momentumJacobian(q, v, update_geometry)
J_am = J_am[3:, :]
L = J_am * v
L_ref, dL_ref, ddL_ref = self._ref_traj(t)
dL_des = dL_ref - self.kp * (L - L_ref)
# Justin's code to compute drift (does not work)
#g = self.robot.bias(q,0*v)
#b = self.robot.bias(q,v)
#b -= g;
#com_p = self.robot.com(q)
#cXi = SE3.Identity()
#oXi = self.robot.data.oMi[1]
#cXi.rotation = oXi.rotation
#cXi.translation = oXi.translation - com_p
#b_com = cXi.actInv(se3.Force(b[:6,0]))
#b_com = b_com.angular;
# Del Prete's quick and dirty way to compute drift
#b_com[:] = 0.0;
# compute momentum Jacobian at next time step assuming zero acc
dt = 1e-3
q_next = se3.integrate(self.robot.model, q, dt * v)
se3.ccrba(self.robot.model, self._data, q_next, v)
J_am_next = self._data.Ag[3:, :]
b_com = (J_am_next - J_am) * v / dt
return J_am[self._mask, :], b_com[self._mask, :], dL_des[self._mask, 0]
def finiteDifferencesdA_dq(model, data, q, v, h):
'''
dAi/dq
'''
q0 = q.copy()
v0 = v.copy()
tensor_dAi_dq = np.zeros((6, model.nv, model.nv))
se3.forwardKinematics(model, data, q0, v0)
se3.computeJacobians(model, data, q0)
pcom_ref = se3.centerOfMass(model, data, q0).copy()
se3.ccrba(model, data, q0, v0)
A0i = data.Ag.copy()
oMc_ref = se3.SE3.Identity() #data.oMi[1].copy()
oMc_ref.translation = pcom_ref #oMc_ref.translation - pcom_ref
for j in range(model.nv):
#vary q
vh = np.matrix(np.zeros((model.nv, 1)))
vh[j] = h
q_integrated = se3.integrate(model, q0.copy(), vh)
se3.forwardKinematics(model, data, q_integrated)
se3.computeJacobians(model, data, q_integrated)
pcom_int = se3.centerOfMass(model, data, q_integrated).copy()
se3.ccrba(model, data, q_integrated, v0)
A0_int = data.Ag.copy()
oMc_int = se3.SE3.Identity() #data.oMi[1].copy()
oMc_int.translation = pcom_int #oMc_int.translation - pcom_int
cMc_int = oMc_ref.inverse() * oMc_int
A0_int = cMc_int.dualAction * A0_int
tensor_dAi_dq[:, :, j] = (A0_int - A0i) / h
return tensor_dAi_dq
def forward_robot(self, q, dq):
# Update the pinocchio model.
self.robot.forwardKinematics(q, dq)
self.robot.computeJointJacobians(q)
self.robot.framesForwardKinematics(q)
se3.ccrba(self.robot.model, self.robot.data, q, dq)
self.last_q = q.copy()
self.last_dq = dq.copy()
def _update_centroidal_quantities(self):
pin.ccrba(self._model, self._data, self._q, self._q_dot)
self._hg = np.zeros_like(self._data.hg)
self._hg[0:3] = np.copy(self._data.hg.angular)
self._hg[3:6] = np.copy(self._data.hg.linear)
self._Ag = np.zeros_like(self._data.Ag)
self._Ag[0:3] = np.copy(self._data.Ag[3:6, :])
self._Ag[3:6] = np.copy(self._data.Ag[0:3, :])
self._Ig = np.zeros_like(self._data.Ig)
self._Ig[0:3, 0:3] = np.copy(self._data.Ig)[3:6, 3:6]
self._Ig[3:6, 3:6] = np.copy(self._data.Ig)[0:3, 0:3]
def dyn_value(self, t, q, v):
#(hg_ref, vhg_ref, ahg_ref) = self._ref_traj(t)
vhg_ref = np.matrix([0., 0., 0., 0., 0., 0.]).T
model = self.robot.model
data = self.robot.data
JMom = se3.ccrba(model, data, q, v)
hg_prv = data.hg.vector.copy()[self._mask,:]
#self.p_error = data.hg.vector.copy()[self._mask,:] - vhg_ref[self._mask,:]
#self.v_error = self.robot.fext[self._mask,:] - vhg_ref[self._mask,:]
#self.v_error = self.robot.fext[self._mask,:]
#self.v_error = data.hg.vector.copy()[self._mask,:] - vhg_ref[self._mask,:]
#self.a_des = -self.kv*self.v_error #+model.gravity.vector[self._mask,:]
self.a_des = self.kv*self.robot.fext[self._mask,:]#vhg_ref #+model.gravity.vector[self._mask,:]
#self.drift = 0 * self.a_des
#self.a_des =
#***********************
p_com = data.com[0]
cXi = SE3.Identity()
oXi = self.robot.data.oMi[1]
cXi.rotation = oXi.rotation
cXi.translation = oXi.translation - p_com
m_gravity = model.gravity.copy()
model.gravity.setZero()
b = se3.nle(model,data,q,v)
model.gravity = m_gravity
f_ff = se3.Force(b[:6])
f_com = cXi.act(f_ff)
hg_drift = f_com.angular
self.drift=f_com.np[self._mask,:]
#************************
#print JMom.copy()[self._mask,:].shape
#print self.__gain_matrix.shape
self._jacobian = JMom.copy()[self._mask,:] * self.__gain_matrix
return self._jacobian, self.drift, self.a_des
def dyn_value(self, t, q, v):
(self.hg_ref, self.dhg_ref, aahg_ref) = self._ref_traj(t)
JMom = se3.ccrba(self.robot.model, self.robot.data, q, v)
b = self._getBiais(q,v)
self.hg_act = self.robot.data.hg.np.A.copy()
self.dhg_act = self._calculateHdot(q)
self.drift=b[self._mask,:]
self.p_error = self.hg_act[self._mask,:] - self.hg_ref[self._mask,:]
self.v_error = self.dhg_act[self._mask,:] - self.dhg_ref[self._mask,:]
self.dh_des = - ((self.kp * self.p_error) + (self.kv*self.v_error) )
self.h_des = - (self.kp * self.p_error)
self._jacobian = JMom.copy()[self._mask,:] * self.__gain_matrix
#self.drift = 0*self.dh_des
return self._jacobian, self.drift, self.h_des
# Get URDF filename
script_dir = os.path.dirname(os.path.realpath(__file__))
pinocchio_model_dir = os.path.join(script_dir, "../urdf")
urdf_filename = pinocchio_model_dir + "/talos_full_legs_v2.urdf"
# Load URDF model
model = pinocchio.buildModelFromUrdf(urdf_filename,
pinocchio.JointModelFreeFlyer())
data = model.createData()
print('model name: ' + model.name)
# Normal configuration (every joint is in origin)
q = pinocchio.neutral(model)
q[7:] = numpy.ndarray((12,), buffer=numpy.array([0.0, 0.028730477193730477, -0.7060605463076763, 1.4774145045100333, \
-0.771353958202357, -0.028730477193733783, 0.0, -0.028730477193724013, -0.7060605463076761, \
1.4774145045100333, -0.7713539582023572, 0.028730477193724013]))
v = pinocchio.utils.zero(model.nv)
# Computes the centroidal mapping, the centroidal momentum and the Centroidal Composite
# Rigid Body Inertia, puts the result in Data and returns the centroidal mapping.
pinocchio.ccrba(model, data, q, v)
# Shows the Inertia Matrix
print('Inertia')
print(data.Ig)
# Computes and shows the center of mass
print('Centre of mass:')
print(pinocchio.centerOfMass(model, data, q))
def update(self, solo, qmes12, vmes12):
"""Update the quantities of the Interface based on the last measurements from the simulation
Args:
solo (object): Pinocchio wrapper for the quadruped
qmes12 (19x1 array): the position/orientation of the trunk and angular position of actuators
vmes12 (18x1 array): the linear/angular velocity of the trunk and angular velocity of actuators
"""
# Rotation matrix from the world frame to the base frame
self.oRb = pin.Quaternion(qmes12[3:7]).matrix()
# Linear and angular velocity in base frame
self.vmes12_base = vmes12.copy()
self.vmes12_base[0:3,
0:1] = self.oRb.transpose() @ self.vmes12_base[0:3,
0:1]
self.vmes12_base[3:6,
0:1] = self.oRb.transpose() @ self.vmes12_base[3:6,
0:1]
"""# Update Kinematics (required or automatically done by other functions?)
pin.forwardKinematics(solo.model, solo.data, qmes12, self.vmes12_base)
self.mean_feet_z = solo.data.oMf[self.indexes[0]].translation[2, 0]
for i in self.indexes[1:]:
self.mean_feet_z = np.min((self.mean_feet_z, solo.data.oMf[i].translation[2, 0]))
qmes12[2, 0] -= self.mean_feet_z"""
# Update Kinematics (required or automatically done by other functions?)
pin.forwardKinematics(solo.model, solo.data, qmes12, self.vmes12_base)
pin.framesForwardKinematics(solo.model, solo.data, qmes12)
# Get center of mass from Pinocchio
pin.centerOfMass(solo.model, solo.data, qmes12, self.vmes12_base)
# Update position/orientation of frames
pin.updateFramePlacements(solo.model, solo.data)
pin.ccrba(solo.model, solo.data, qmes12, self.vmes12_base)
# Update minimum height of feet
# TODO: Rename mean_feet_z into min_feet_z
self.mean_feet_z = solo.data.oMf[self.indexes[0]].translation[2, 0]
"""for i in self.indexes:
self.mean_feet_z += solo.data.oMf[i].translation[2, 0]
self.mean_feet_z *= 0.25"""
for i in self.indexes[1:]:
self.mean_feet_z = np.min(
(self.mean_feet_z, solo.data.oMf[i].translation[2, 0]))
self.mean_feet_z = 0.0
# Store position, linear velocity and angular velocity in global frame
self.oC = solo.data.com[0]
self.oV = solo.data.vcom[0]
self.oW = vmes12[3:6]
pin.crba(solo.model, solo.data, qmes12)
# Get SE3 object from world frame to base frame
self.oMb = pin.SE3(pin.Quaternion(qmes12[3:7]), self.oC)
self.RPY = pin.rpy.matrixToRpy(self.oMb.rotation)
# Get SE3 object from world frame to local frame
self.oMl = pin.SE3(
pin.utils.rotate('z', self.RPY[2, 0]),
np.array([qmes12[0, 0], qmes12[1, 0], self.mean_feet_z]))
# Get position, linear velocity and angular velocity in local frame
self.lC = self.oMl.inverse() * self.oC
self.lV = self.oMl.rotation.transpose() @ self.oV
self.lW = self.oMl.rotation.transpose() @ self.oW
# Pos, vel and acc of feet
for i, j in enumerate(self.indexes):
# Position of feet in local frame
self.o_feet[:, i:(i + 1)] = solo.data.oMf[j].translation
self.l_feet[:, i:(
i + 1)] = self.oMl.inverse() * solo.data.oMf[j].translation
# getFrameVelocity output is in the frame of the foot so a transform is required
self.ov_feet[:, i:(
i + 1)] = solo.data.oMf[j].rotation @ pin.getFrameVelocity(
solo.model, solo.data, j).vector[0:3, 0:1]
self.lv_feet[:, i:(
i + 1
)] = self.oMl.rotation.transpose() @ self.ov_feet[:, i:(i + 1)]
# getFrameAcceleration output is in the frame of the foot so a transform is required
self.oa_feet[:, i:(
i + 1)] = solo.data.oMf[j].rotation @ pin.getFrameAcceleration(
solo.model, solo.data, j).vector[0:3, 0:1]
self.la_feet[:, i:(
i + 1
)] = self.oMl.rotation.transpose() @ self.oa_feet[:, i:(i + 1)]
# Orientation of the base in local frame
# Base and local frames have the same yaw orientation in world frame
self.abg[0:2] = self.RPY[0:2]
# Position of shoulders in world frame
for i in range(4):
self.o_shoulders[:, i:(i + 1)] = self.oMl * self.l_shoulders[:, i]
return 0
# EXERCIZE 2: Compute robot kinetic energy #get a random generalized velocity v = rand(model.nv) # Update the joint and frame placements pin.forwardKinematics(model,data,q,v) # compute using generalized velocities and system mass matrix # ..... ## compare with pinocchio native function pin.computeKineticEnergy(model,data,q,v) #print "TEST2: ", EkinRobot - data.kinetic_energy ##EXERCISE 3: Build the transformation matrix to use com coordinates # get the location of the base frame w_base = data.oMi[1].translation #compute centroidal quantitities (hg, Ag and Ig) pin.ccrba(model, data, q, v) # G_T_B = ... # EXERCISE 4: Check the mass matrix becomes block diagonal if you appy the tranform # M_g = ... #print "\n The mass matrix expressed at the com becomes diagonal: \n", M_g # EXERSISE 5: Check that joint gravity vector nullifies # Gg = ...
def cam(self, q, vel):
hg = []
for i in range(0, len(q)):
se3.ccrba(self.model, self.data, q[i], vel[i])
hg.append(self.data.hg.copy())
return hg