def appendContactForcesTrajs(first_iter_for_phase=False):
"""
Append the current contact force value to the current phase, for all the effectors in contact
:param first_iter_for_phase: if True, initialize a new Curve for the phase contact trajectories
"""
for eeName in phase.effectorsInContact():
contact = dic_contacts[eeName]
if invdyn.checkContact(contact.name, sol):
contact_forces = invdyn.getContactForce(contact.name, sol)
contact_normal_force = np.array(
contact.getNormalForce(contact_forces))
else:
logger.warning(
"invdyn check contact returned false while the reference contact is active !"
)
contact_normal_force = np.zeros(1)
if cfg.Robot.cType == "_3_DOF":
contact_forces = np.zeros(3)
else:
contact_forces = np.zeros(12)
if first_iter_for_phase:
phase.addContactForceTrajectory(
eeName,
piecewise(polynomial(contact_forces.reshape(-1, 1), t, t)))
phase.addContactNormalForceTrajectory(
eeName,
piecewise(
polynomial(contact_normal_force.reshape(1, 1), t, t)))
else:
phase.contactForce(eeName).append(contact_forces, t)
phase.contactNormalForce(eeName).append(
contact_normal_force.reshape(1), t)
def setCOMfromCurve(phase, curve_normalized):
"""
Initialise the phase c_t dc_t and ddc_t from the given curve,
Also set the final values for the CoM from the final points of the curve
Also set or increase the final time from the duration of the curve
:param phase: the ContactPhase to modify
:param curve_normalized: the curve is defined in [0,max], we need to shift it
"""
if phase.c_t is None:
phase.c_t = piecewise()
phase.dc_t = piecewise()
phase.ddc_t = piecewise()
t_min = phase.timeInitial
# set initial values
phase.c_init = curve_normalized(0.)
phase.dc_init = curve_normalized.derivate(0., 1)
phase.ddc_init = curve_normalized.derivate(0., 2)
else:
t_min = phase.c_t.max()
t_max = t_min + curve_normalized.max()
# shift the curve to the correct initial time :
curve = bezier(curve_normalized.waypoints(), t_min, t_max)
# set the trajectory in the phase :
phase.c_t.append(curve)
phase.dc_t.append(curve.compute_derivate(1))
phase.ddc_t.append(curve.compute_derivate(2))
# set the new final values :
phase.timeFinal = t_max
phase.c_final = phase.c_t(t_max)
phase.dc_final = phase.dc_t(t_max)
phase.ddc_final = phase.ddc_t(t_max)
def appendCentroidal(first_iter_for_phase = False):
"""
Compute the values of the CoM position, velocity, acceleration, the anuglar momentum and it's derivative
from the wholebody data and append them to the current phase trajectories
:param first_iter_for_phase: if True, set the initial values for the current phase
and initialize the centroidal trajectories
"""
pcom, vcom, acom = pinRobot.com(q, v, dv)
L = pinRobot.centroidalMomentum(q, v).angular
dL = pin.computeCentroidalMomentumTimeVariation(pinRobot.model, pinRobot.data, q, v, dv).angular
if first_iter_for_phase:
phase.c_init = pcom
phase.dc_init = vcom
phase.ddc_init = acom
phase.L_init = L
phase.dL_init = dL
phase.c_t = piecewise(polynomial(pcom.reshape(-1,1), t , t))
phase.dc_t = piecewise(polynomial(vcom.reshape(-1,1), t, t))
phase.ddc_t = piecewise(polynomial(acom.reshape(-1,1), t, t))
phase.L_t = piecewise(polynomial(L.reshape(-1,1), t, t))
phase.dL_t = piecewise(polynomial(dL.reshape(-1,1), t, t))
else:
phase.c_t.append(pcom, t)
phase.dc_t.append(vcom, t)
phase.ddc_t.append(acom, t)
phase.L_t.append(L, t)
phase.dL_t.append(dL, t)
def initEmptyPhaseWholeBodyTrajectory(phase):
"""
Initialize wholebody trajectories with empty piecewise curves
:param phase: the ContactPhase
"""
phase.q_t = piecewise()
phase.dq_t = piecewise()
phase.ddq_t = piecewise()
phase.tau_t = piecewise()
def initEmptyPhaseCentroidalTrajectory(phase):
"""
Initialize centroidal trajectories with empty piecewise curves
:param phase: the ContactPhase
"""
phase.c_t = piecewise()
phase.dc_t = piecewise()
phase.ddc_t = piecewise()
phase.L_t = piecewise()
phase.dL_t = piecewise()
def appendJointsDerivatives(first_iter_for_phase=False):
"""
Append the current v and dv value to the current phase.dq_t and phase.ddq_t trajectory
:param first_iter_for_phase: if True, initialize a new Curve for phase.dq_t and phase.ddq_t
"""
if first_iter_for_phase:
phase.dq_t = piecewise(polynomial(v.reshape(-1, 1), t, t))
phase.ddq_t = piecewise(polynomial(dv.reshape(-1, 1), t, t))
else:
phase.dq_t.append(v, t)
phase.ddq_t.append(dv, t)
def connectPhaseTrajToFinalState(phase, duration = None):
"""
Append to the trajectory of c, dc and ddc a quintic spline connecting phase.c_final, dc_final and ddc_final
and L and dL with a trajectory at 0
:param phase: the contact phase to modify
:param duration: the duration of the new trajectories. If not provided, the duration is computed from the last
point in the current trajectories and phase.timeFinal
"""
if phase.c_t is None or phase.dc_t is None or phase.ddc_t is None:
# initialise empty trajectories
phase.c_t = piecewise()
phase.dc_t = piecewise()
phase.ddc_t = piecewise()
# get the initial state from the phase :
c_init = phase.c_init
dc_init = phase.dc_init
ddc_init = phase.ddc_init
t_init = phase.timeInitial
else:
# get the initial state from the last points of the trajectories :
t_init = phase.c_t.max()
c_init = phase.c_t(t_init)
dc_init = phase.dc_t(t_init)
ddc_init = phase.ddc_t(t_init)
if phase.L_t is None or phase.dL_t is None :
# initialise empty trajectories
phase.L_t = piecewise()
phase.dL_t = piecewise()
# get the initial state from the phase :
L_init = phase.L_init
dL_init = phase.dL_init
else :
L_init = phase.c_t(t_init)
dL_init = phase.dL_t(t_init)
if not phase.c_final.any():
raise RuntimeError("connectPhaseTrajToFinalState can only be called with a phase with an initialized c_final")
if duration is not None:
t_final = t_init + duration
else:
t_final = phase.timeFinal
com_t = polynomial(c_init, dc_init, ddc_init, phase.c_final, phase.dc_final, phase.ddc_final, t_init, t_final)
L_t = polynomial(L_init, dL_init, phase.L_final, phase.dL_final, t_init, t_final)
phase.c_t.append(com_t)
phase.dc_t.append(com_t.compute_derivate(1))
phase.ddc_t.append(com_t.compute_derivate(2))
phase.L_t.append(L_t)
phase.dL_t.append(L_t.compute_derivate(1))
phase.timeFinal = t_final
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 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 connectPhaseTrajToInitialState(phase, duration):
"""
Insert at the beginning of the trajectory of c, dc and ddc a quintic spline connecting phase.c_init, dc_init and ddc__init
and L and dL with a trajectory at 0
:param phase: the contact phase to modify
:param duration: the duration of the new trajectories.
"""
if duration <= 0.:
return
if phase.c_t is None or phase.dc_t is None or phase.ddc_t is None:
raise RuntimeError("connectPhaseTrajToFinalState can only be called with a phase with an initialized COM trajectory")
if phase.L_t is None or phase.dL_t is None :
raise RuntimeError("connectPhaseTrajToFinalState can only be called with a phase with an initialized AM trajectory")
if not phase.c_init.any():
raise RuntimeError("connectPhaseTrajToFinalState can only be called with a phase with an initialized c_final")
t_final = phase.c_t.min()
t_init = t_final - duration
c_final = phase.c_t(t_final)
dc_final = phase.dc_t(t_final)
ddc_final = phase.ddc_t(t_final)
L_final = phase.L_t(t_final)
dL_final = phase.dL_t(t_final)
com_t = polynomial( phase.c_init, phase.dc_init, phase.ddc_init,c_final, dc_final, ddc_final, t_init, t_final)
L_t = polynomial(phase.L_init, phase.dL_init, L_final, dL_final, t_init, t_final)
# insert this trajectories at the beginning of the phase :
piecewise_c= piecewise(com_t)
piecewise_c.append(phase.c_t)
phase.c_t = piecewise_c
piecewise_dc= piecewise(com_t.compute_derivate(1))
piecewise_dc.append(phase.dc_t)
phase.dc_t = piecewise_dc
piecewise_ddc= piecewise(com_t.compute_derivate(2))
piecewise_ddc.append(phase.ddc_t)
phase.ddc_t = piecewise_ddc
piecewise_L= piecewise(L_t)
piecewise_L.append(phase.L_t)
phase.L_t = piecewise_L
piecewise_dL= piecewise(L_t.compute_derivate(1))
piecewise_dL.append(phase.dL_t)
phase.dL_t = piecewise_dL
# set the new initial time
phase.timeInitial = t_init
def appendTorques(first_iter_for_phase = False):
"""
Append the current tau value to the current phase.tau_t trajectory
:param first_iter_for_phase: if True, initialize a new Curve for phase.tau_t
"""
tau = invdyn.getActuatorForces(sol)
if first_iter_for_phase:
phase.tau_t = piecewise(polynomial(tau.reshape(-1,1), t, t))
else:
phase.tau_t.append(tau, t)
def computeZMPFromWrench(cs, Robot, dt):
for phase in cs.contactPhases:
t = phase.timeInitial
phase.zmp_t = None
while t <= phase.timeFinal:
phi0 = Force(phase.wrench_t(t))
zmp = shiftZMPtoFloorAltitude(cs, t, phi0, Robot)
if phase.zmp_t is None:
phase.zmp_t = piecewise(polynomial(zmp.reshape(-1, 1), t, t))
else:
phase.zmp_t.append(zmp, t)
t += dt
if phase.timeFinal - dt / 2. <= t <= phase.timeFinal + dt / 2.:
t = phase.timeFinal
def appendJointsValues(first_iter_for_phase = False):
"""
Append the current q value to the current phase.q_t trajectory
:param first_iter_for_phase: if True, set the current phase.q_init value
and initialize a new Curve for phase.q_t
"""
if first_iter_for_phase:
phase.q_init = q
phase.q_t = piecewise(polynomial(q.reshape(-1,1), t, t))
#phase.root_t = piecewise_SE3(constantSE3curve(SE3FromConfig(q) ,t))
else:
phase.q_t.append(q, t)
#phase.root_t.append(SE3FromConfig(q), t)
if queue_qt:
queue_qt.put([phase.q_t.curve_at_index(phase.q_t.num_curves()-1),
phase.dq_t.curve_at_index(phase.dq_t.num_curves()-1),
None,
False])
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
dc1 = zeros(3)
ddc1 = zeros(3)
c_t_right = polynomial(c0, dc0, ddc0, c1, dc1, ddc1, t, t+2)
t = c_t_right.max()
c0 = c1.copy()
# o above the left feet in 3 seconds:
c1 = phase.contactPatch('leg_left_sole_fix_joint').placement.translation
c1[2] = c0[2]
c_t_left = polynomial(c0, dc0, ddc0, c1, dc1, ddc1, t, t+3)
t = c_t_left.max()
c0 = c1.copy()
# go back to the initial CoM position in 2 seconds
c1 = com
c_t_mid = polynomial(c0, dc0, ddc0, c1, dc1, ddc1, t, t+2)
c_t = piecewise(c_t_right)
c_t.append(c_t_left)
c_t.append(c_t_mid)
phase.c_t = c_t
phase.dc_t = c_t.compute_derivate(1)
phase.ddc_t = c_t.compute_derivate(2)
# set the timings of the phase :
phase.timeInitial = c_t.min()
phase.timeFinal = c_t.max()
# set a 0 AM trajectory
generateZeroAMreference(cs)
assert cs.haveTimings()
