def computeThroughFootWrenches(self, wrench_lf, wrench_rf):
w_X_lf = self.robot.data.oMi[self.robot.legs['lf_foot']]
w_X_rf = self.robot.data.oMi[self.robot.legs['rf_foot']]
self.wrench_lf_W = w_X_lf.act(pin.Force(wrench_lf[:3], wrench_lf[3:]))
self.wrench_rf_W = w_X_rf.act(pin.Force(wrench_rf[:3], wrench_rf[3:]))
self.wrench_T = (self.wrench_lf_W + self.wrench_rf_W).vector
return self.computeCoPFromWrench(self.wrench_T)
def _getContribution(self,com,s):
'''
Get segment contribution to centroidal angular
momenta and its rate of change
Inputs:
- s : segment index
- com : position of the CoM in world reference frame
'''
# get spatial inertia and velocity
Y = self.robot.model.inertias[s]
V = self.robot.data.v[s]
# centroidal momenta in body frame
# ihi = I Vi
iHi = se3.Force(Y.matrix()*V.vector)
# rate of change of centroidal momenta in body frame
# ih_doti = Iai + Vi x* hi
# TO VERIFY
iHDi = (self.robot.model.inertias[s]*self.robot.data.a[s]) + se3.Inertia.vxiv(Y,V)
#iHDi.linear
# express at the center of mass
oMc = se3.SE3.Identity()
oMc.translation = self.robot.data.oMi[1].translation - com
# uncomment in case need to change the rotation of the reference frame
oMc.rotation = self.robot.data.oMi[1].rotation
cMi = oMc.actInv( self.robot.data.oMi[s] )
cHi = cMi.act(iHi).np.A1
cHDi = cMi.act(iHDi).np.A1
return cHi, cHDi
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 updateState(robot, q, v, sensor_data):
# Get dcm from current state
pin.forwardKinematics(robot.pinocchio_model_th, robot.pinocchio_data_th, q,
v)
comOut = pin.centerOfMass(robot.pinocchio_model_th,
robot.pinocchio_data_th, q, v)
vcomOut = robot.pinocchio_data_th.vcom[0]
dcmOut = comOut + vcomOut / omega
# Create zmp from forces
forces = np.asarray(sensor_data[ForceSensor.type])
newWrench = pin.Force.Zero()
for i, name in enumerate(contact_points):
update_frame(robot.pinocchio_model_th, robot.pinocchio_data_th, name)
placement = get_frame_placement(robot.pinocchio_model_th,
robot.pinocchio_data_th, name)
wrench = pin.Force(np.array([0.0, 0.0, forces[2, i]]), np.zeros(3))
newWrench += placement.act(wrench)
totalWrenchOut = newWrench
if totalWrenchOut.linear[2] > 0:
zmpOut = [
-totalWrenchOut.angular[1] / totalWrenchOut.linear[2],
totalWrenchOut.angular[0] / totalWrenchOut.linear[2]
]
else:
zmpOut = zmp_log
return comOut, vcomOut, dcmOut, zmpOut, totalWrenchOut
def f12TOphi(f12, points=contact_Point):
phi = pin.Force.Zero()
for i in range(4):
phii = pin.Force(f12[i * 3:i * 3 + 3], np.zeros(3))
fMi = pin.SE3(np.eye(3), points[:, i])
phi += fMi.act(phii)
return phi
def dyn_value(self, t, q, v, update_geometry=False):
g = self.robot.bias(q, 0 * v)
b = self.robot.bias(q, v)
b -= g
M = self.robot.mass(q)
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 = cXi.actInv(b[:6,0]).vector
b_com = b_com.angular
# M_com = cXi.inverse().action.T * M[:6,:]
# M_com = cXi.inverse().np.T * M[:6,:]
M_com = self.robot.momentumJacobian(q, v, update_geometry)
M_com = M_com[3:, :]
L = M_com * v
L_ref, dL_ref, ddL_ref = self._ref_traj(t)
# acc = dL_ref - b_com[3:,:]
dL_des = dL_ref - self.kp * (L - L_ref)
return M_com[self._mask, :], b_com[self._mask, :], dL_des[self._mask,
0]
def computeWrench(cs, Robot, dt):
rp = RosPack()
urdf = rp.get_path(
Robot.packageName
) + '/urdf/' + Robot.urdfName + Robot.urdfSuffix + '.urdf'
#srdf = "package://" + package + '/srdf/' + cfg.Robot.urdfName+cfg.Robot.srdfSuffix + '.srdf'
robot = RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(),
pin.JointModelFreeFlyer(), False)
model = robot.model
data = robot.data
q_t = cs.concatenateQtrajectories()
dq_t = cs.concatenateQtrajectories()
ddq_t = cs.concatenateQtrajectories()
t = q_t.min()
for phase in cs.contactPhases:
phase.wrench_t = None
t = phase.timeInitial
while t <= phase.timeFinal:
pin.rnea(model, data, phase.q_t(t), phase.dq_t(t), phase.ddq_t(t))
pcom, vcom, acom = robot.com(phase.q_t(t), phase.dq_t(t),
phase.ddq_t(t))
# FIXME : why do I need to call com to have the correct values in data ??
phi0 = data.oMi[1].act(pin.Force(data.tau[:6]))
if phase.wrench_t is None:
phase.wrench_t = piecewise(
polynomial(phi0.vector.reshape(-1, 1), t, t))
else:
phase.wrench_t.append(phi0.vector, t)
t += dt
if phase.timeFinal - dt / 2. <= t <= phase.timeFinal + dt / 2.:
t = phase.timeFinal
def writeFromMessage(self, msg):
t = msg.time
q = np.zeros(self._model.nq)
v = np.zeros(self._model.nv)
tau = np.zeros(self._model.njoints - 2)
p = dict()
pd = dict()
f = dict()
s = dict()
# Retrieve the generalized position and velocity, and joint torques
q[3] = msg.centroidal.base_orientation.x
q[4] = msg.centroidal.base_orientation.y
q[5] = msg.centroidal.base_orientation.z
q[6] = msg.centroidal.base_orientation.w
v[3] = msg.centroidal.base_angular_velocity.x
v[4] = msg.centroidal.base_angular_velocity.y
v[5] = msg.centroidal.base_angular_velocity.z
for j in range(len(msg.joints)):
jointId = self._model.getJointId(msg.joints[j].name) - 2
q[jointId + 7] = msg.joints[j].position
v[jointId + 6] = msg.joints[j].velocity
tau[jointId] = msg.joints[j].effort
pinocchio.centerOfMass(self._model, self._data, q, v)
q[0] = msg.centroidal.com_position.x - self._data.com[0][0]
q[1] = msg.centroidal.com_position.y - self._data.com[0][1]
q[2] = msg.centroidal.com_position.z - self._data.com[0][2]
v[0] = msg.centroidal.com_velocity.x - self._data.vcom[0][0]
v[1] = msg.centroidal.com_velocity.y - self._data.vcom[0][1]
v[2] = msg.centroidal.com_velocity.z - self._data.vcom[0][2]
# Retrive the contact information
for contact in msg.contacts:
name = contact.name
# Contact pose
pose = contact.pose
position = np.array(
[pose.position.x, pose.position.y, pose.position.z])
quaternion = pinocchio.Quaternion(pose.orientation.w,
pose.orientation.x,
pose.orientation.y,
pose.orientation.z)
p[name] = pinocchio.SE3(quaternion, position)
# Contact velocity
velocity = contact.velocity
lin_vel = np.array(
[velocity.linear.x, velocity.linear.y, velocity.linear.z])
ang_vel = np.array(
[velocity.angular.x, velocity.angular.y, velocity.angular.z])
pd[name] = pinocchio.Motion(lin_vel, ang_vel)
# Contact wrench
wrench = contact.wrench
force = np.array([wrench.force.x, wrench.force.y, wrench.force.z])
torque = np.array(
[wrench.torque.x, wrench.torque.y, wrench.torque.z])
f[name] = [contact.type, pinocchio.Force(force, torque)]
# Surface normal and friction coefficient
normal = contact.surface_normal
nsurf = np.array([normal.x, normal.y, normal.z])
s[name] = [nsurf, contact.friction_coefficient]
return t, q, v, tau, p, pd, f, s
def test_conversion(self):
f = pin.Force.Random()
f_array = np.array(f)
f_from_array = pin.Force(f_array)
self.assertTrue(f_from_array == f)
def step(self, q, vq, tauq, dt=None):
if dt is None: dt = self.dt
robot = self.robot
NQ, NV, NB, RF, LF, RK, LK = self.NQ, self.NV, self.NB, self.RF, self.LF, self.RK, self.LK
# --- Simu
se3.forwardKinematics(robot.model, robot.data, q, v)
se3.framesKinematics(robot.model, robot.data)
Mrf = self.Mrf0.inverse() * robot.data.oMf[RF]
vrf = robot.model.frames[RF].placement.inverse() * robot.data.v[RK]
Mlf = self.Mlf0.inverse() * robot.data.oMf[LF]
vlf = robot.model.frames[LF].placement.inverse() * robot.data.v[LK]
qrf = np.vstack(
[Mrf.translation[1:],
se3.rpy.matrixToRpy(Mrf.rotation)[0]])
qlf = np.vstack(
[Mlf.translation[1:],
se3.rpy.matrixToRpy(Mlf.rotation)[0]])
frf = self.frf # Buffer where to store right force
frf[[1, 2, 3]] = self.Krf * qrf # Right force in effector frame
rf0_frf = se3.Force(frf) # Force in rf0 frame
rk_frf = (robot.data.oMi[RK].inverse() * self.Mrf0).act(
rf0_frf) # Spring force in R-knee frame.
flf = self.flf # Buffer where to store left force
flf[[1, 2, 3]] = self.Klf * qlf # Left force in effector frame
lf0_flf = se3.Force(flf) # Force in lf0 frame
lk_flf = (robot.data.oMi[LK].inverse() * self.Mlf0).act(
lf0_flf) # Spring force in L-knee frame.
self.forces = ForceDict({
RK: rk_frf,
LK: lk_flf
}, NB) # Argument to ABA
aq = se3.aba(robot.model, robot.data, q, vq, tauq, self.forces)
vq += aq * dt
q = se3.integrate(robot.model, q, vq * dt)
self.aq = aq
return q, vq
def test_se3_action(self):
f = se3.Force.Random()
m = se3.SE3.Random()
self.assertTrue(
np.allclose((m * f).vector,
np.linalg.inv(m.action.T) * f.vector))
self.assertTrue(
np.allclose((m.actInv(f)).vector, m.action.T * f.vector))
v = se3.Motion.Random()
f = se3.Force(np.vstack([v.vector[3:], v.vector[:3]]))
self.assertTrue(np.allclose((v**f).vector, zero(6)))
def test_force(self):
m = self.m
self.assertApprox(pin.Force.Zero().vector, zero(6))
f = pin.Force.Random()
ff = f.linear
ft = f.angular
self.assertApprox(f.vector, np.concatenate([ff, ft]))
self.assertApprox((m * f).vector, npl.inv(m.action.T).dot(f.vector))
self.assertApprox((m.actInv(f)).vector, m.action.T.dot(f.vector))
v = pin.Motion.Random()
f = pin.Force(np.concatenate([v.vector[3:], v.vector[:3]]))
self.assertApprox((v ^ f).vector, zero(6))
def test_force(self):
m = self.m
self.assertApprox(se3.Force.Zero().vector, zero(6))
f = se3.Force.Random()
ff = f.linear
ft = f.angular
self.assertApprox(f.vector, np.vstack([ff, ft]))
self.assertApprox((m * f).vector, npl.inv(m.action.T) * f.vector)
self.assertApprox((m.actInv(f)).vector, m.action.T * f.vector)
v = se3.Motion.Random()
f = se3.Force(np.vstack([v.vector[3:], v.vector[:3]]))
self.assertApprox((v ^ f).vector, zero(6))
def setForces(self, data, forcesArr, forcesVec=None):
'''
Convert a numpy array of forces into a stdVector of spatial forces.
Side effect: keep the force values in data.
'''
# In the dynamic equation, we wrote M*a + J.T*fdyn, while in the ABA it would be
# M*a + b = tau + J.T faba, so faba = -fdyn (note the minus operator before a2m).
data.f = forcesArr
if forcesVec is None:
forcesVec = data.forces
data.forces[data.joint] *= 0
forcesVec[data.joint] += data.jMf * pinocchio.Force(a2m(forcesArr))
return forcesVec
def constraint_dyn(self, x, nc=0):
'''
M aq + b(q,vq) + g(q) = tau_q + J(q)^T f
= rnea(q,vq=0,aq=0,fs)
'''
q, tauq, fr, fl = self.x2qf(x)
# Forces should be stored in RNEA-compatible structure
forces = pinocchio.StdVect_Force()
for i in range(self.rmodel.njoints):
forces.append(pinocchio.Force.Zero())
# Forces should be expressed at the joint for RNEA, while we store them
# at the frame in the optim problem. Convert.
jMr = rmodel.frames[self.idR].placement
forces[self.jidR] = jMr * pinocchio.Force(fr)
jMl = rmodel.frames[self.idL].placement
forces[self.jidL] = jMl * pinocchio.Force(fl)
#q = self.rmodel.neutralConfiguration
aq = vq = zero(self.rmodel.nv)
tauref = pinocchio.rnea(self.rmodel, self.rdata, q, vq, aq, forces)
self.eq[nc:nc + self.rmodel.nv] = (tauref - tauq).flat
return self.eq[nc:nc + self.rmodel.nv].tolist()
def computeThroughFootCoPs(self, wrench_lf, wrench_rf):
self.wrench_LF = wrench_lf
self.wrench_RF = wrench_rf
cop_lf = self.computeCoPFromWrench(self.wrench_LF)
cop_rf = self.computeCoPFromWrench(self.wrench_RF)
# Expressing the local CoP position into the world frame
w_X_lf = self.robot.data.oMi[self.robot.legs['lf_foot']]
w_X_rf = self.robot.data.oMi[self.robot.legs['rf_foot']]
cop_lf_W = w_X_lf * cop_lf
cop_rf_W = w_X_rf * cop_rf
self.wrench_LF_W = w_X_lf.act(pin.Force(wrench_lf[:3],
wrench_lf[3:])).vector
self.wrench_RF_W = w_X_lf.act(pin.Force(wrench_rf[:3],
wrench_rf[3:])).vector
# Computing the CoP of the system, i.e.
# p = (f^lf_z * p^lf + f^rf_z * p^rf) / (f^lf_z + f^rf_z)
fz_lf = np.asscalar(self.wrench_LF_W[2])
fz_rf = np.asscalar(self.wrench_RF_W[2])
self.cop = (fz_lf * cop_lf_W + fz_rf * cop_rf_W) / (fz_lf + fz_rf)
return self.cop
def computeCoPFromWrench(self, wrench):
cop = np.matrix([0., 0., 0.]).T
# self.FOOT_FORCE_SENSOR_XYZ = np.matrix([0.0, 0.0, -0.085]).T
# sole_X_ftSens = pin.SE3(np.eye(3), -self.FOOT_FORCE_SENSOR_XYZ);
sole_X_ftSens = pin.SE3(np.eye(3),
np.matrix([0., 0., 0.]).T)
wrench = pin.Force(wrench[:3], wrench[3:])
wrench = sole_X_ftSens.act(wrench)
# The CoP is defined as the point where the pressure force moments
# vanishes. This occurs when tau = [p]x f, which the 6d force (i.e.
# wrench) is [f,tau]. It also equivalents to: p = [n]x tau / (f.n),
# which simplifies is p = [-tau_y, tau_x] / f_z
cop[0] = -wrench.angular[1] / wrench.linear[2]
cop[1] = wrench.angular[0] / wrench.linear[2]
return cop
def computeWrench(res):
rp = RosPack()
urdf = rp.get_path(
cfg.Robot.packageName
) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf'
#srdf = "package://" + package + '/srdf/' + cfg.Robot.urdfName+cfg.Robot.srdfSuffix + '.srdf'
robot = RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(),
pin.JointModelFreeFlyer(), False)
model = robot.model
data = robot.data
for k in range(res.N):
pin.rnea(model, data, res.q_t[:, k], res.dq_t[:, k], res.ddq_t[:, k])
pcom, vcom, acom = robot.com(
res.q_t[:, k], res.dq_t[:, k], res.ddq_t[:, k]
) # FIXME : why do I need to call com to have the correct values in data ??
phi0 = data.oMi[1].act(pin.Force(data.tau[:6]))
res.wrench_t[:, k] = phi0.vector
return res
def _getBiais(self,q,v):
model = self.robot.model
data = self.robot.data
p_com = self.robot.com(q)
oXi = data.oMi[1]
cXi = se3.SE3.Identity()
cXi.rotation = oXi.rotation
cXi.translation = oXi.translation - p_com
#cXi.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)
return f_com.np
def TransformForcesToJointLocalFrames(self,
contact_forces,
plate_contact_force_map,
contact_force_joint_map,
save_result=False):
'''
Express each contact force w.r.t. the "local frame" of the contact
point, i.e. the coordinate frame whose origin overlaps the point
of contact and whose axis are in parallel with the world frame.
'''
self.contact_force_joint_map = contact_force_joint_map
oM_forceplates = self.ComputeForceplateDisplacements()
contact_forces_local = np.zeros(
(self.num_frames_in_video, self.num_contact_forces, 6))
for k in range(self.num_contact_forces):
# Get contact joint id and force plate id using contact force id
joint_id = contact_force_joint_map[k]
forceplate_ids = []
for fpid, fid in enumerate(plate_contact_force_map):
# fpid: force platform id
# fid: contact force id
if fid == k + 1:
forceplate_ids.append(fpid)
if len(forceplate_ids) == 0:
continue
for i in range(self.num_frames_in_video):
p_c = self.joint_3d_positions[i, joint_id, :] # in meters
force_wrt_c = se3.Force(zero(6))
for fpid in forceplate_ids:
# Compute forceplate displacement wrt person joint
cR_plate = oM_forceplates[fpid].rotation.copy()
p_plate = oM_forceplates[fpid].translation.copy()
if fpid in [0, 1]: # force plates on the floor
relative_position = p_plate - np.matrix(p_c).T
cM_plate = se3.SE3(cR_plate, relative_position)
elif fpid in [2, 3]: # force sensors on the shelf
cM_plate = se3.SE3(cR_plate, zero(3))
else:
raise ValueError("Should never happen!")
# Express the 6D force k in person joint's local frame
force_wrt_plate = se3.Force(
np.matrix(self.contact_forces[i, fpid, :]).T)
force_wrt_c += cM_plate.act(force_wrt_plate)
contact_forces_local[i, k, :] = \
force_wrt_c.vector.getA().reshape(-1)
self.contact_forces_local = contact_forces_local
# save the results to file
if save_result:
results = {
"contact_forces_local": self.contact_forces_local,
"contact_forces_names": self.contact_force_names,
"fps": self.fps_video
}
if hasattr(self, "mask_uncaptured_forces"):
results["mask_uncaptured_forces"] = \
self.mask_uncaptured_forces
save_path = join(self.parkour_dataset, "gt_motion_forces",
"contact_forces_local.pkl")
check_and_save_data(results, save_path, self.video_name)
print(' Contact force saved to {}'.format(save_path))
b = pinocchio.rnea(model, data, q, v, zero(model.nv)).copy()
M = pinocchio.crba(model, data, q).copy()
# ahat = a.l + wxv
pinocchio.forwardKinematics(model, data, q, v, zero(model.nv))
gamma = data.a[-1].linear + cross(data.v[-1].angular, data.v[-1].linear)
# M a + b = tau + J'f
# J a = -gamma
K = np.bmat([[M, J.T], [J, zero([3, 3])]])
r = np.concatenate([tau - b, -gamma])
af = inv(K) * r
a = af[:model.nv]
f = af[model.nv:]
fs[-1] = pinocchio.Force(-f, zero(3))
assert (absmax(rnea(model, data, q, v, a, fs) - (M * a + b + J.T * f)) < 1e-6)
assert (
absmax(aba(model, data, q, v, tau, fs) - (inv(M) *
(tau - b - J.T * f))) < 1e-6)
# Check contact-dyninv deriv
# af = Ki r = [ MJt; J0 ] [ tau-b;-gamma ]
# af' = -Ki K' Ki r + Ki r'
# = -Ki ( K' af - r' )
# = -Ki [ M'a + Jt'f + b' ; J' a + gamma' ]
# Check M'a + J'f + b'
dtau_dqn = df_dq(model, lambda _q: rnea(model, data, _q, v, a, fs), q)
dtau_dvn = df_dq(model, lambda _v: rnea(model, data, q, _v, a, fs), v)
print(
'NOTE: "predicted" wrench values are not accurate due to the foot saturation and as such they are not checked.'
)
print(
"CoP values are predicted under the assumption that they are at the foot border and as such they are checked."
)
# --- Wrench saturation (left) ---
print()
print("--- Wrench saturation (left) ---")
distribute.phase.value = 1
distribute.phase.time = 1
wrenchLeft = wrench
ankleWrenchLeft = list(
leftPos.actInv(pin.Force(np.matrix(wrenchLeft).T)).vector.flat)
print("expected global wrench: %s" % str(wrench))
print("expected global left wrench: %s" % str(wrenchLeft))
print("expected ankle left wrench: %s" % str(ankleWrenchLeft))
copLeft = [
float(com[0] - leftPos.translation[0]),
distribute_conf.RIGHT_FOOT_SIZES[3], 0.
]
print("expected sole left CoP: %s" % str(copLeft))
print()
distribute.zmpRef.recompute(1)
def computeResultTrajectory(robot, t_t, q_t, v_t, a_t):
model = robot.model
data = robot.data
N = q_t.shape[1]
ZMP_t = np.matrix(np.zeros([3, N]))
waist_t = np.matrix(np.zeros([3, N]))
pcom_t = np.matrix(np.empty([3, N]))
vcom_t = np.matrix(np.empty([3, N]))
acom_t = np.matrix(np.empty([3, N]))
tau_t = np.matrix(np.empty([robot.nv, N]))
wrench_t = np.matrix(np.empty([6, N]))
waist_orientation_t = np.matrix(np.empty([3, N]))
# Sample end effector traj
ee_t = dict()
RF_t = []
ee_t["rf"] = (RF_t)
LF_t = []
ee_t["lf"] = (LF_t)
RH_t = []
ee_t["rh"] = (RH_t)
LH_t = []
ee_t["lh"] = (LH_t)
Mee = dict()
Mee["rf"] = robot.Mrf
Mee["lf"] = robot.Mlf
Mee["rh"] = robot.Mrh
Mee["lh"] = robot.Mlh
ee_names = list(ee_t.viewkeys())
for k in range(N):
q = q_t[:, k]
v = v_t[:, k]
a = a_t[:, k]
#M = robot.mass(q)
#b = robot.biais(q,v)
#robot.dynamics(q,v,0*v)
se3.rnea(model, data, q, v, a)
#se3.forwardKinematics(model,data,q)
#robot.computeJacobians(q)
pcom, vcom, acom = robot.com(q, v, a)
# Update EE placements
for ee in ee_names:
ee_t[ee].append(Mee[ee](q, update_kinematics=False).copy())
# Update CoM data
pcom_t[:, k] = pcom
vcom_t[:, k] = vcom
acom_t[:, k] = acom
#oXi_s = robot.data.oMi[1].inverse().np.T
#phi0 = oXi_s * (M[:6,:] * a + b[:6])
tau_t[:, k] = data.tau
phi0 = data.oMi[1].act(se3.Force(data.tau[:6]))
wrench_t[:, k] = phi0.vector
forces = wrench_t[:3, k]
torques = wrench_t[3:, k]
ZMP_t[0, k] = -torques[1] / forces[2]
ZMP_t[1, k] = torques[0] / forces[2]
waist_t[:, k] = robot.data.oMi[1].translation
waist_orientation_t[:, k] = matrixToRpy(robot.data.oMi[1].rotation)
result = Struct()
result.t_t = t_t
result.ZMP_t = ZMP_t
result.waist_t = waist_t
result.waist_orientation_t = waist_orientation_t
result.pcom_t = pcom_t
result.vcom_t = vcom_t
result.acom_t = acom_t
result.wrench_t = wrench_t
result.tau_t = tau_t
result.q_t = q_t
result.v_t = v_t
result.a_t = a_t
result.ee_t = ee_t
return result
stop = distribute.emergencyStop.value
np.testing.assert_equal(stop, 0)
# --- Wrench saturation (left) ---
print()
print("--- Wrench saturation ---")
print('NOTE: no actual "saturation" is performed. As such, the resulting CoP may be outside the support area')
# --- Wrench saturation (left) ---
print()
print("--- Wrench saturation (left) ---")
distribute.phase.value = 1
distribute.phase.time = 1
wrenchLeft = wrench
ankleWrenchLeft = list(leftPos.actInv(pin.Force(np.matrix(wrenchLeft).T)).vector.flat)
print("expected global wrench: %s" % str(wrench))
print("expected global left wrench: %s" % str(wrenchLeft))
print("expected ankle left wrench: %s" % str(ankleWrenchLeft))
copLeft = [float(com[0] - leftPos.translation[0]), float(com[1] - leftPos.translation[1]), 0.]
print("expected sole left CoP: %s" % str(copLeft))
print()
distribute.zmpRef.recompute(1)
print("resulting global wrench: %s" % str(distribute.wrenchRef.value))
assertApprox(wrench, distribute.wrenchRef.value, 2)
print("resulting global left wrench: %s" % str(distribute.wrenchLeft.value))
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)
assert ((c_hc - c_T_b * b_hc).isZero())
# Check that the linear momentum is m*cd
pio.centerOfMass(rm, rd, q, vq)
cd = rd.vcom[0]
m = rd.mass[0]
np.allclose(c_hc.linear, m * cd)
# Transforms vq to "centroid vel configuration vect" in universe frame
Z = np.vstack([
Jc,
np.hstack([np.zeros([3, 3]), w_T_b.rotation,
np.zeros([3, rm.nv - 6])]),
np.hstack([np.zeros([rm.nv - 6, 6]),
np.eye(rm.nv - 6)])
cy = []
for d in data:
q = config_sot_to_urdf(np.asmatrix(d.q))
wrench_lf = d.f_lf
wrench_rf = d.f_rf
# Evaluation of the center of pressure from different methods
hrp2.update(q)
p = cop.compute(wrench_lf, wrench_rf, 'FootWrenches')
c = pin.centerOfMass(hrp2.model, hrp2.data, q)
print 'foot wrenches', cop.compute(wrench_lf, wrench_rf, 'FootWrenches').T
print 'foot cops ', cop.compute(wrench_lf, wrench_rf, 'FootCoPs').T
wrench_lf_s = hrp2.compute_sMa(wrench_lf).act(
pin.Force(wrench_lf[:3], wrench_lf[3:])).vector
wrench_rf_s = hrp2.compute_sMa(wrench_rf).act(
pin.Force(wrench_rf[:3], wrench_rf[3:])).vector
wrench_t = wrench_lf_s + wrench_rf_s
mass = np.linalg.norm(wrench_t[:3]) / 9.81
print 'mass ', mass
print '---'
cx.append(np.asscalar(c[0]))
cy.append(np.asscalar(c[1]))
px.append(np.asscalar(p[0]))
py.append(np.asscalar(p[1]))
#import pinocchio as pin
#print pin.centerOfMass(hrp2.model, hrp2.data, q).T
x = np.arange(-0.02, 0.02, 0.001)
assert(isapprox(m.actInv(p),npl.inv(m.action())*p)) # --- Motion assert(isapprox(se3.Motion.Zero().vector(),zero(6))) v = se3.Motion.Random() assert(isapprox((m*v).vector(),m.action()*v.vector())) assert(isapprox((m.actInv(v)).vector(),npl.inv(m.action())*v.vector())) vv = v.linear; vw = v.angular assert(isapprox( v.vector(),np.vstack([vv,vw]) )) assert(isapprox((v**v).vector(),zero(6))) # --- Force --- assert(isapprox(se3.Force.Zero().vector(),zero(6))) f = se3.Force.Random() ff = f.linear; ft = f.angular assert(isapprox( f.vector(),np.vstack([ff,ft]) )) assert(isapprox((m*f).vector(),npl.inv(m.action().T)*f.vector())) assert(isapprox((m.actInv(f)).vector(),m.action().T*f.vector())) f = se3.Force( np.vstack([ v.vector()[3:], v.vector()[:3] ]) ) assert(isapprox( (v**f).vector(),zero(6) )) # --- Inertia --- Y1 = se3.Inertia.Random() Y2 = se3.Inertia.Random() Y=Y1+Y2 assert(isapprox(Y1.matrix()+Y2.matrix(),Y.matrix())) assert(isapprox((Y*v).vector(),Y.matrix()*v.vector())) assert(isapprox( (m*Y).matrix(),m.inverse().action().T*Y.matrix()*m.inverse().action())) assert(isapprox( (m.actInv(Y)).matrix(),m.action().T*Y.matrix()*m.action()))
skew(rand(3)) # Skew "cross-product" 3x3 matrix from a 3x1 vector
cross(rand(3), rand(3)) # Cross product of R^3
rotate('x', 0.4) # Build a rotation matrix of 0.4rad around X.
import pinocchio as se3
R = eye(3)
p = zero(3)
M0 = se3.SE3(R, p)
M = se3.SE3.Random()
M.translation = p
M.rotation = R
v = zero(3)
w = zero(3)
nu0 = se3.Motion(v, w)
nu = se3.Motion.Random()
nu.linear = v
nu.angular = w
f = zero(3)
tau = zero(3)
phi0 = se3.Force(f, tau)
phi = se3.Force.Random()
phi.linear = f
phi.angular = tau
from pinocchio.robot_wrapper import RobotWrapper
from os.path import join
PKG = '/home/alumno04/dev_samir/ros/sawyer_ws/src/'
URDF = join(PKG, '/sawyer_robot/sawyer_description/urdf/model1.urdf')
robot = RobotWrapper(URDF, [PKG])
