def appendZMP(first_iter_for_phase=False):
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]))
wrench = phi0.vector
zmp = shiftZMPtoFloorAltitude(cs, t, phi0, cfg.Robot)
if first_iter_for_phase:
phase.zmp_t = piecewise(polynomial(zmp.reshape(-1, 1), t, t))
phase.wrench_t = piecewise(polynomial(wrench.reshape(-1, 1), t, t))
else:
phase.zmp_t.append(zmp, t)
phase.wrench_t.append(wrench, t)
def appendZMP(first_iter_for_phase = False):
"""
Compute the zmp from the current wholebody data and append it to the current phase
:param first_iter_for_phase: if True, initialize a new Curve for phase.zmp_t
"""
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]))
wrench = phi0.vector
zmp = shiftZMPtoFloorAltitude(cs, t, phi0, cfg.Robot)
if first_iter_for_phase:
phase.zmp_t = piecewise(polynomial(zmp.reshape(-1,1), t, t))
phase.wrench_t = piecewise(polynomial(wrench.reshape(-1,1), t, t))
else:
phase.zmp_t.append(zmp, t)
phase.wrench_t.append(wrench, t)
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