def computeWrenchRef(res):
Mcom = SE3.Identity()
for k, t in enumerate(res.t_t):
Mcom.translation = res.c_reference[:, k]
Fcom = Force.Zero()
Fcom.linear = cfg.MASS * (res.ddc_reference[:, k] - cfg.GRAVITY)
Fcom.angular = res.dL_reference[:, k]
F0 = Mcom.act(Fcom)
res.wrench_reference[:, k] = F0.vector
return res
def computeWrenchRef(cs, mass, G, dt):
Mcom = SE3.Identity()
for phase in cs.contactPhases:
t = phase.timeInitial
phase.wrench_t = None
while t <= phase.timeFinal:
Mcom.translation = phase.c_t(t)
Fcom = Force.Zero()
Fcom.linear = mass * (phase.ddc_t(t) - G)
Fcom.angular = phase.dL_t(t)
F0 = Mcom.act(Fcom)
if phase.wrench_t is None:
phase.wrench_t = piecewise(
polynomial(F0.vector.reshape(-1, 1), t, t))
else:
phase.wrench_t.append(F0.vector, t)
t += dt
if phase.timeFinal - dt / 2. <= t <= phase.timeFinal + dt / 2.:
t = phase.timeFinal
def storeData(k_t, res, q, v, dv, invdyn, sol):
# store current state
res.q_t[:, k_t] = q
res.dq_t[:, k_t] = v
res.ddq_t[:, k_t] = dv
res.tau_t[:, k_t] = invdyn.getActuatorForces(
sol) # actuator forces, with external forces (contact forces)
#store contact info (force and status)
if cfg.IK_store_contact_forces:
for eeName, contact in dic_contacts.iteritems():
if invdyn.checkContact(contact.name, sol):
res.contact_forces[eeName][:,
k_t] = invdyn.getContactForce(
contact.name, sol)
res.contact_normal_force[
eeName][:, k_t] = contact.getNormalForce(
res.contact_forces[eeName][:, k_t])
res.contact_activity[eeName][:, k_t] = 1
# store centroidal info (real one and reference) :
if cfg.IK_store_centroidal:
pcom, vcom, acom = pinRobot.com(q, v, dv)
res.c_t[:, k_t] = pcom
res.dc_t[:, k_t] = vcom
res.ddc_t[:, k_t] = acom
res.L_t[:, k_t] = pinRobot.centroidalMomentum(q, v).angular
#res.dL_t[:,k_t] = pinRobot.centroidalMomentumVariation(q,v,dv) # FIXME : in robot wrapper, use * instead of .dot() for np matrices
pin.dccrba(pinRobot.model, pinRobot.data, q, v)
res.dL_t[:, k_t] = Force(
pinRobot.data.Ag.dot(dv) + pinRobot.data.dAg.dot(v)).angular
# same for reference data :
res.c_reference[:, k_t] = com_desired
res.dc_reference[:, k_t] = vcom_desired
res.ddc_reference[:, k_t] = acom_desired
res.L_reference[:, k_t] = L_desired
res.dL_reference[:, k_t] = dL_desired
if cfg.IK_store_zmp:
tau = pin.rnea(
pinRobot.model, pinRobot.data, q, v, dv
) # tau without external forces, only used for the 6 first
#res.tau_t[:6,k_t] = tau[:6]
phi0 = pinRobot.data.oMi[1].act(Force(tau[:6]))
res.wrench_t[:, k_t] = phi0.vector
res.zmp_t[:, k_t] = shiftZMPtoFloorAltitude(
cs, res.t_t[k_t], phi0, cfg.EXPORT_OPENHRP)
# same but from the 'reference' values :
Mcom = SE3.Identity()
Mcom.translation = com_desired
Fcom = Force.Zero()
Fcom.linear = cfg.MASS * (acom_desired - cfg.GRAVITY)
Fcom.angular = dL_desired
F0 = Mcom.act(Fcom)
res.wrench_reference[:, k_t] = F0.vector
res.zmp_reference[:, k_t] = shiftZMPtoFloorAltitude(
cs, res.t_t[k_t], F0, cfg.EXPORT_OPENHRP)
if cfg.IK_store_effector:
for eeName in usedEffectors: # real position (not reference)
res.effector_trajectories[eeName][:, k_t] = SE3toVec(
getCurrentEffectorPosition(robot, invdyn.data(), eeName))
res.d_effector_trajectories[eeName][:, k_t] = MotiontoVec(
getCurrentEffectorVelocity(robot, invdyn.data(), eeName))
res.dd_effector_trajectories[eeName][:, k_t] = MotiontoVec(
getCurrentEffectorAcceleration(robot, invdyn.data(),
eeName))
return res