def appendEffectorsTraj(first_iter_for_phase = False):
"""
Append the current position of the effectors not in contact to the current phase trajectories
:param first_iter_for_phase: if True, initialize a new Curve for the phase effector trajectories
"""
if first_iter_for_phase and phase_prev:
for eeName in phase_prev.effectorsWithTrajectory():
if t > phase_prev.effectorTrajectory(eeName).max():
placement = getCurrentEffectorPosition(robot, invdyn.data(), eeName)
phase_prev.effectorTrajectory(eeName).append(placement, t)
for eeName in phase.effectorsWithTrajectory():
placement = getCurrentEffectorPosition(robot, invdyn.data(), eeName)
if first_iter_for_phase:
phase.addEffectorTrajectory(eeName, piecewise_SE3(constantSE3curve(placement, t)))
else:
phase.effectorTrajectory(eeName).append(placement, t)
def appendEffectorsTraj(first_iter_for_phase=False):
if first_iter_for_phase and phase_prev:
for eeName in phase_prev.effectorsWithTrajectory():
if t > phase_prev.effectorTrajectory(eeName).max():
placement = getCurrentEffectorPosition(
robot, invdyn.data(), eeName)
phase_prev.effectorTrajectory(eeName).append(placement, t)
if first_iter_for_phase:
for eeName in phase.effectorsWithTrajectory():
placement = getCurrentEffectorPosition(robot, invdyn.data(),
eeName)
phase.addEffectorTrajectory(
eeName, piecewise_SE3(constantSE3curve(placement, t)))
else:
for eeName in phase.effectorsWithTrajectory():
placement = getCurrentEffectorPosition(robot, invdyn.data(),
eeName)
phase.effectorTrajectory(eeName).append(placement, t)
def FromContactSequenceWB(cs_ref, cs, dt):
p0 = cs.contactPhases[0]
nq = p0.q_t.dim()
nv = p0.dq_t.dim()
nu = p0.tau_t.dim()
eeNames = cs.getAllEffectorsInContact()
t_begin = p0.timeInitial
res = Result(nq, nv, dt, eeNames, cs=cs, t_begin=t_begin, nu=nu)
# fill all the array with discretization of the curves
res.q_t = discretizeCurve(cs.concatenateQtrajectories(), dt)[0]
res.dq_t = discretizeCurve(cs.concatenateDQtrajectories(), dt)[0]
res.ddq_t = discretizeCurve(cs.concatenateDDQtrajectories(), dt)[0]
res.tau_t = discretizeCurve(cs.concatenateTauTrajectories(), dt)[0]
res.c_t = discretizeCurve(cs.concatenateCtrajectories(), dt)[0]
res.dc_t = discretizeCurve(cs.concatenateDCtrajectories(), dt)[0]
res.ddc_t = discretizeCurve(cs.concatenateDDCtrajectories(), dt)[0]
res.L_t = discretizeCurve(cs.concatenateLtrajectories(), dt)[0]
res.dL_t = discretizeCurve(cs.concatenateDLtrajectories(), dt)[0]
res.c_reference = discretizeCurve(cs_ref.concatenateCtrajectories(), dt)[0]
res.dc_reference = discretizeCurve(cs_ref.concatenateDCtrajectories(),
dt)[0]
res.ddc_reference = discretizeCurve(cs_ref.concatenateDDCtrajectories(),
dt)[0]
res.L_reference = discretizeCurve(cs_ref.concatenateLtrajectories(), dt)[0]
res.dL_reference = discretizeCurve(cs_ref.concatenateDLtrajectories(),
dt)[0]
res.zmp_t = discretizeCurve(cs.concatenateZMPtrajectories(), dt)[0]
res.wrench_t = discretizeCurve(cs.concatenateWrenchTrajectories(), dt)[0]
res.zmp_reference = discretizeCurve(cs_ref.concatenateZMPtrajectories(),
dt)[0]
res.wrench_reference = discretizeCurve(
cs_ref.concatenateWrenchTrajectories(), dt)[0]
root_t = cs_ref.concatenateRootTrajectories()
res.waist_orientation_reference = discretizeSE3CurveQuaternion(root_t,
dt)[0]
res.d_waist_orientation_reference = discretizeDerivateCurve(root_t, dt,
1)[0][3:, :]
res.dd_waist_orientation_reference = discretizeDerivateCurve(
root_t, dt, 2)[0][3:, :]
for ee in eeNames:
res.contact_forces[ee] = discretizeCurve(
cs.concatenateContactForceTrajectories(ee), dt)[0]
res.contact_normal_force[ee] = discretizeCurve(
cs.concatenateNormalForceTrajectories(ee), dt)[0]
eff = cs.concatenateEffectorTrajectories(ee)
# append init/end constant trajectorie if required :
if eff.min() < t_begin:
eff_begin = constantSE3curve(eff.evaluateAsSE3(eff.min()), t_begin,
eff.min())
eff_mid = eff
eff = piecewise_SE3(eff_begin)
eff.append(eff_mid)
if eff.max() < cs.contactPhases[-1].timeFinal:
eff_end = constantSE3curve(eff.evaluateAsSE3(eff.max()), eff.max(),
cs.contactPhases[-1].timeFinal)
eff.append(eff_end)
res.effector_trajectories[ee] = discretizeSE3CurveToVec(eff, dt)[0]
res.d_effector_trajectories[ee] = discretizeDerivateCurve(eff, dt,
1)[0]
res.dd_effector_trajectories[ee] = discretizeDerivateCurve(eff, dt,
2)[0]
eff = cs_ref.concatenateEffectorTrajectories(ee)
# append init/end constant trajectorie if required :
if eff.min() < t_begin:
eff_begin = constantSE3curve(eff.evaluateAsSE3(eff.min()), t_begin,
eff.min())
eff_mid = eff
eff = piecewise_SE3(eff_begin)
eff.append(eff_mid)
if eff.max() < cs.contactPhases[-1].timeFinal:
eff_end = constantSE3curve(eff.evaluateAsSE3(eff.max()), eff.max(),
cs.contactPhases[-1].timeFinal)
eff.append(eff_end)
res.effector_references[ee] = discretizeSE3CurveToVec(eff, dt)[0]
res.d_effector_references[ee] = discretizeDerivateCurve(eff, dt, 1)[0]
res.dd_effector_references[ee] = discretizeDerivateCurve(eff, dt, 2)[0]
return res