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 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 generateZeroAMreference(cs):
"""
For all phases in the sequence, set an L_t and dL_t trajectory constant at 0 with the correct duration
:param cs: the contact sequence to modify
"""
for phase in cs.contactPhases:
phase.L_t = polynomial(np.zeros(3), np.zeros(3), phase.timeInitial, phase.timeFinal)
phase.dL_t = polynomial(np.zeros(3), np.zeros(3), phase.timeInitial, phase.timeFinal)
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 compute_wholebody(self, robot, cs_com, last_q=None, last_v=None, last_iter=False):
"""
Compute the wholebody motion for the given ContactSequence
:param robot: a TSID RobotWrapper instance
:param cs_com: a ContactSequence with centroidal trajectories
:param last_q: the last wholebody configuration (used as Initial configuration for this iteration)
:param last_v: the last joint velocities vector (used as initial joint velocities for this iteration)
:param last_iter: if True, the complete ContactSequence is used, if False only the first 2 phases are used
:return: a ContactSequence with wholebody data, the last wholebody configuration, the last joint velocity,
the last phase, the TSID RobotWrapper used
"""
if not self.EffectorInputs.checkAndFillRequirements(cs_com, self.cfg, self.fullBody):
raise RuntimeError(
"The current contact sequence cannot be given as input to the end effector method selected.")
# Generate end effector trajectories for the contactSequence
cs_ref_full = self.generate_effector_trajectories(self.cfg, cs_com, self.fullBody)
# EffectorOutputs.assertRequirements(cs_ref_full)
# Split contactSequence if it is not the last iteration
if last_iter:
cs_ref = cs_ref_full
last_phase = cs_com.contactPhases[-1]
else:
cs_cut = ContactSequence()
for i in range(2):
cs_cut.append(cs_ref_full.contactPhases[i])
cs_ref = cs_cut
# last_phase should be a double support phase, it should contains all the contact data:
last_phase = cs_com.contactPhases[2]
tools.setFinalFromInitialValues(last_phase, last_phase)
if last_q is not None:
cs_ref.contactPhases[0].q_init = last_q
if last_v is not None:
t_init = cs_ref.contactPhases[0].timeInitial
cs_ref.contactPhases[0].dq_t = polynomial(last_v.reshape(-1, 1), t_init, t_init)
### Generate the wholebody trajectory:
update_root_traj_timings(cs_ref)
if not self.WholebodyInputs.checkAndFillRequirements(cs_ref, self.cfg, self.fullBody):
raise RuntimeError(
"The current contact sequence cannot be given as input to the wholeBody method selected.")
cs_wb, robot = self.generate_wholebody(self.cfg, cs_ref, robot=robot)
logger.info("-- compute whole body END")
# WholebodyOutputs.assertRequirements(cs_wb)
# Retrieve the last phase, q, and v from this outputs:
last_phase_wb = cs_wb.contactPhases[-1]
last_q = last_phase_wb.q_t(last_phase_wb.timeFinal)
last_v = last_phase_wb.dq_t(last_phase_wb.timeFinal)
tools.deletePhaseCentroidalTrajectories(last_phase) # Remove unnecessary data to reduce serialized size
last_phase.q_final = last_q
last_phase.dq_t = polynomial(last_v.reshape(-1, 1), last_phase.timeFinal, last_phase.timeFinal)
#last_phase.c_final = last_phase_wb.c_final
#last_phase.dc_final = last_phase_wb.dc_final
#last_phase.L_final = last_phase_wb.L_final
return cs_wb, last_q, last_v, last_phase, robot
def genAMTrajFromPhaseStates(phase, constraintVelocity = True):
"""
Generate a cubic spline connecting (L_init, dL_init) to (L_final, dL_final) and set it as the phase AM trajectory
:param phase: the ContactPhase to use
:param constraintVelocity: if False, generate only a linear interpolation and ignore the values of dL
"""
if constraintVelocity:
am_traj = polynomial(phase.L_init, phase.dL_init, phase.L_final, phase.dL_final,
phase.timeInitial, phase.timeFinal)
else:
am_traj = polynomial(phase.L_init, phase.L_final, phase.timeInitial, phase.timeFinal)
phase.L_t = am_traj
phase.dL_t = am_traj.compute_derivate(1)
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 genCOMTrajFromPhaseStates(phase, constraintVelocity = True, constraintAcceleration = True):
"""
Generate a quintic spline connecting exactly (c, dc, ddc) init to final
:param phase:
:param constraintVelocity: if False, generate only a linear interpolation and ignore ddc, and dc values
:param constraintAcceleration: if False, generate only a cubic spline and ignore ddc values
"""
if constraintAcceleration and not constraintVelocity:
raise ValueError("Cannot constraints acceleration if velocity is not constrained.")
if constraintAcceleration:
com_traj = polynomial(phase.c_init, phase.dc_init, phase.ddc_init,
phase.c_final, phase.dc_final, phase.ddc_final,phase.timeInitial, phase.timeFinal)
elif constraintVelocity:
com_traj = polynomial(phase.c_init, phase.dc_init, phase.c_final, phase.dc_final,
phase.timeInitial, phase.timeFinal)
else:
com_traj = polynomial(phase.c_init, phase.c_final, phase.timeInitial, phase.timeFinal)
phase.c_t = com_traj
phase.dc_t = com_traj.compute_derivate(1)
phase.ddc_t = com_traj.compute_derivate(2)
def constantSE3curve(placement, t_min, t_max = None):
"""
Create a constant SE3_curve at the given placement for the given duration
:param placement: the placement
:param t_min: the initial time
:param t_max: final time, if not provided the curve will have a duration of 0
:return: the constant curve
"""
if t_max is None:
t_max = t_min
rot = SO3Linear(placement.rotation, placement.rotation, t_min, t_max)
trans = polynomial(placement.translation.reshape(-1,1), t_min, t_max)
return SE3Curve(trans, rot)
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
com = array(fb.getCenterOfMass())
phase.c_init = com
phase.c_final = com
phase.q_final = phase.q_init
# generate com motion :
t = 0
# first go above the right feet in 2 seconds
c0 = com.copy()
dc0 = zeros(3)
ddc0 = zeros(3)
c1 = phase.contactPatch('leg_right_sole_fix_joint').placement.translation
c1[2] = c0[2] - 0.03 # go down 3cm to avoid kinematic limits
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)
