def compute_from_cs(self):
"""
Split the complete ContactSequence stored in self.cs and send each subsequence in the pipe_cs
"""
pid_centroidal = 0
last_iter_centroidal = False
logger.info("## Compute from cs, size = %d", self.cs.size())
last_phase = self.get_last_phase()
if last_phase:
tools.setFinalFromInitialValues(last_phase, self.cs.contactPhases[0])
while pid_centroidal + 5 < self.cs.size():
logger.debug("## Current pid = %d", pid_centroidal)
if pid_centroidal + 7 >= self.cs.size():
logger.debug("## Last centroidal iter")
# last iter, take all the remaining phases
num_phase = self.cs.size() - pid_centroidal
last_iter_centroidal = True
else:
num_phase = 5
logger.debug("## Num phase = %d", num_phase)
# Extract the phases [pid_centroidal; pid_centroidal +num_phases] from cs_full
cs_iter = ContactSequence(0)
for i in range(pid_centroidal, pid_centroidal + num_phase):
logger.debug("-- Add phase : %d", i)
cs_iter.append(self.cs.contactPhases[i])
self.pipe_cs_in.send([cs_iter, last_iter_centroidal]) # This call may be blocking if the pipe is full
pid_centroidal += 2
self.pipe_cs_in.close()
def test_step_in_place(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "step_in_place.cs"))
self.assertEqual(cs.size(), 9)
self.assertTrue(cs.haveConsistentContacts())
self.assertFalse(cs.haveFriction())
self.assertFalse(cs.haveContactModelDefined())
def create_stairs_cs():
ENV_NAME = "multicontact/bauzil_stairs"
fb, v = display_tools.initScene(Robot, ENV_NAME, False)
cs = ContactSequence(0)
# Create an initial contact phase :
q_ref = fb.referenceConfig[::] + [0] * 6
q_ref[0:2] = [0.07, 1.2]
addPhaseFromConfig(fb, cs, q_ref, [fb.rLegId, fb.lLegId])
step_height = 0.1
step_width = 0.3
displacement = SE3.Identity()
displacement.translation = array([step_width, 0, step_height])
cs.moveEffectorOf(fb.rfoot, displacement)
cs.moveEffectorOf(fb.lfoot, displacement)
q_end = q_ref[::]
q_end[0] += step_width
q_end[2] += step_height
fb.setCurrentConfig(q_end)
com = fb.getCenterOfMass()
setFinalState(cs, array(com), q=q_end)
return cs
def generate_effector_trajectories_for_sequence(cfg, cs, generate_end_effector_traj, fullBody = None):
"""
Generate an effector trajectory for each effectors which are going to be in contact in the next phase
:param cfg: an instance of the configuration class
:param cs: the contact sequence
:param generate_end_effector_traj: a pointer to the method used to generate an end effector trajectory for one phase
:param fullBody: an instance of rbprm FullBody
:return: a new contact sequence, containing the same data as the one given as input
plus the effector trajectories for each swing phases
"""
cs_res = ContactSequence(cs)
effectors = cs_res.getAllEffectorsInContact()
previous_phase = None
for pid in range(cs_res.size()-1): # -1 as last phase never have effector trajectories
phase = cs_res.contactPhases[pid]
next_phase = cs_res.contactPhases[pid+1]
if pid > 0 :
previous_phase = cs_res.contactPhases[pid-1]
for eeName in effectors:
if not phase.isEffectorInContact(eeName) and next_phase.isEffectorInContact(eeName):
# eeName will be in compute in the next phase, a trajectory should be added in the current phase
placement_end = next_phase.contactPatch(eeName).placement
time_interval = [phase.timeInitial, phase.timeFinal]
if previous_phase is not None and previous_phase.isEffectorInContact(eeName):
placement_init = previous_phase.contactPatch(eeName).placement
else:
placement_init = effectorPlacementFromPhaseConfig(phase,eeName,fullBody)
# build the trajectory :
traj = generate_end_effector_traj(cfg, time_interval,placement_init,placement_end)
phase.addEffectorTrajectory(eeName,traj)
return cs_res
def generate_contact_sequence_load(cfg):
fb, v = display_tools.initScene(cfg.Robot, cfg.ENV_NAME)
cs = ContactSequence(0)
print("Import contact sequence binary file : ", cfg.CS_FILENAME)
cs.loadFromBinary(cfg.CS_FILENAME)
display_tools.displayWBconfig(v, cs.contactPhases[0].q_init)
return cs, fb, v
def generate_effector_trajectories_for_sequence_load(cfg, cs, fullBody=None):
cs_ref = ContactSequence()
filename = cfg.REF_FILENAME
print("Load contact sequence with end effector trajectories from : ",
filename)
cs_ref.loadFromBinary(filename)
return cs_ref
def test_walk_20cm_quasistatic(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic.cs"))
self.assertEqual(cs.size(), 23)
self.assertFalse(cs.haveFriction())
self.assertFalse(cs.haveContactModelDefined())
self.assertTrue(cs.haveConsistentContacts())
def generateCentroidalTrajectory(cs,
cs_initGuess=None,
fullBody=None,
viewer=None):
cs_res = ContactSequence(0)
cs_res.loadFromBinary(cfg.COM_FILENAME)
print("Import contact sequence binary file : ", cfg.COM_FILENAME)
return cs_res
def test_com_motion_above_feet_COM(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "com_motion_above_feet_COM.cs"))
self.assertEqual(cs.size(), 1)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
checkCS(self, cs)
def generate_wholebody_load(cfg, cs, fullBody=None, viewer=None):
rp = RosPack()
urdf = rp.get_path(cfg.Robot.packageName) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf'
logger.info("load robot : %s", urdf)
robot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer(), False)
logger.info("robot loaded.")
cs_wb = ContactSequence()
logger.warning("Load wholebody contact sequence from file : %s", cfg.WB_FILENAME)
cs_wb.loadFromBinary(cfg.WB_FILENAME)
return cs_wb, robot
def test_walk_20cm_quasistatic_COM(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_COM.cs"))
self.assertEqual(cs.size(), 23)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertFalse(cs.haveFriction())
self.assertFalse(cs.haveContactModelDefined())
checkCS(self, cs, quasistatic=True, effector=False, wholeBody=False)
def generate_centroidal_load(cfg,
cs,
cs_initGuess=None,
fullBody=None,
viewer=None,
first_iter=True):
cs_res = ContactSequence(0)
cs_res.loadFromBinary(cfg.COM_FILENAME)
logger.warning("Import contact sequence binary file : %s",
cfg.COM_FILENAME)
return cs_res
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 contactSequenceFromRBPRMConfigs(fb, beginId, endId):
"""
Build a multicontact_api ContactSequence from rbprm states list
:param fb: an instance of rbprm.FullBody
:param beginId: the first state Id
:param endId: the last state Id
:return: a multicontact_api ContactSequence
"""
logger.warning("generate contact sequence from planning : ")
n_states = endId - beginId + 1
# There could be either contact break, creation or repositionning between each adjacent states.
# But there should be only contacts break or creation between each adjacent contactPhases
cs = ContactSequence(0)
prev_phase = None
phaseId = 0
# create initial ContactPhase
cs.append(createPhaseFromRBPRMState(fb, beginId))
for stateId in range(beginId + 1,
endId + 1): # from the second state to the last one
logger.info("current state id = %d", stateId)
previous_phase = cs.contactPhases[-1]
eeName, variationType = getContactVariationBetweenStates(
fb, stateId - 1, stateId)
if eeName is not None:
if variationType == VariationType.REPOSITIONNED:
# in case of repositionning, the centroidal motion will happend in the next intermediate phase,
# and thus the previous_phase will not move :
copyPhaseInitToFinal(previous_phase)
else:
# set the final values of the previous phase to be the current one :
setPhaseFinalValues(fb, stateId, previous_phase)
if variationType == VariationType.BROKEN:
# remove the given contact :
cs.breakContact(eeName)
else:
# get placement of the new contact from the planning :
contact_placement = contactPlacementFromRBPRMState(
fb, stateId, eeName)
if variationType == VariationType.CREATED:
# create a new contact :
cs.createContact(eeName, ContactPatch(contact_placement))
elif variationType == VariationType.REPOSITIONNED:
# move existing contact to new placement :
cs.moveEffectorToPlacement(eeName, contact_placement)
# set the initial values for the current phase from planning :
setPhaseInitialValues(fb, stateId, cs.contactPhases[-1])
# set the same values as the final ones for the intermediate state created :
setPhaseFinalValues(fb, stateId, cs.contactPhases[-2])
# else : no contact changes, ignore this state
setPhaseFinalValues(fb, endId, cs.contactPhases[-1])
return cs
def test_com_motion_above_feet_WB(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "com_motion_above_feet_WB.cs"))
self.assertEqual(cs.size(), 1)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertTrue(cs.haveJointsTrajectories())
self.assertTrue(cs.haveJointsDerivativesTrajectories())
self.assertTrue(cs.haveContactForcesTrajectories())
self.assertTrue(cs.haveZMPtrajectories())
checkCS(self, cs, wholeBody=True)
def generateCentroidalTrajectory(cs,
cs_initGuess=None,
fullBody=None,
viewer=None,
first_iter=True):
"""
Generate straight line trajectories from the center of the support polygon of one phase
to the center in the next phase.
Compute trajectories as Quintic splines, starting and ending with null acceleration and velocity at each phase change
Add a null angular momentum.
:param cs:
:param cs_initGuess:
:param fullBody:
:param viewer:
:return:
"""
if cs_initGuess:
print("WARNING : in centroidal.geometric, initial guess is ignored.")
if not first_iter:
print(
"WARNING : in centroidal.geometric, it is useless to iterate several times."
)
if cs.haveCOMvalues():
# do not overwrite com values in input sequence
cs_result = ContactSequence(cs)
computePhasesCOMValues(cs_result,
cfg.Robot.DEFAULT_COM_HEIGHT,
overwrite=True)
else:
# add them in output sequence if not present
computePhasesCOMValues(cs, cfg.Robot.DEFAULT_COM_HEIGHT)
cs_result = ContactSequence(cs)
for pid, phase in enumerate(cs_result.contactPhases):
genCOMTrajFromPhaseStates(phase)
genAMTrajFromPhaseStates(phase)
return cs_result
def test_step_in_place_REF(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "step_in_place_REF.cs"))
self.assertEqual(cs.size(), 9)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories(1e-6, False))
self.assertTrue(cs.haveFriction())
self.assertTrue(cs.haveContactModelDefined())
checkCS(self, cs, root=True, effector=True, wholeBody=False)
def generate_centroidal_quasistatic(cfg,
cs,
cs_initGuess=None,
fullBody=None,
viewer=None,
first_iter=True):
if cs_initGuess:
logger.warning("Initial guess is ignored.")
if not fullBody:
raise ValueError("quasiStatic called without fullBody object.")
if not first_iter:
logger.warning("It is useless to iterate several times.")
beginId, endId = createFullbodyStatesFromCS(cs, fullBody)
logger.info("beginid = %d", beginId)
logger.info("endId = %d", endId)
cs_result = ContactSequence(cs)
# for each phase in the cs, create a corresponding FullBody State and call 2-PAC,
# then fill the cs struct with a quintic spline connecting the two points found
id_phase = 0
for id_state in range(beginId, endId + 1):
logger.debug("id_state = %d", id_state)
logger.debug("id_phase = %d", id_phase)
phase_fixed = cs_result.contactPhases[
id_phase] # phase where the CoM move and the contacts are fixed
# set initial state to be the final one of the previous phase :
if id_phase > 1:
setInitialFromFinalValues(phase_swing, phase_fixed)
# compute 'optimal' position of the COM to go before switching phase:
if id_state == endId:
c = phase_fixed.c_final
else:
c = getTargetCOMPosition(fullBody, id_state, cfg.COM_SHIFT_Z)
# set 'c' the final position of current phase :
phase_fixed.c_final = c
phase_fixed.dc_final = np.zeros(3)
phase_fixed.ddc_final = np.zeros(3)
connectPhaseTrajToFinalState(phase_fixed)
id_phase += 1
if id_state < endId:
phase_swing = cs_result.contactPhases[
id_phase] # phase where the CoM is fixed and an effector move
# in swing phase, com do not move :
setInitialFromFinalValues(phase_fixed, phase_swing)
copyPhaseInitToFinal(phase_swing)
connectPhaseTrajToFinalState(phase_swing)
id_phase += 1
return cs_result
def test_walk_20cm_quasistatic_WB(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_WB.cs"))
self.assertEqual(cs.size(), 23)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories(1e-1))
self.assertTrue(cs.haveJointsTrajectories())
self.assertTrue(cs.haveJointsDerivativesTrajectories())
self.assertTrue(cs.haveContactForcesTrajectories())
self.assertTrue(cs.haveZMPtrajectories())
self.assertTrue(cs.haveFriction())
self.assertTrue(cs.haveContactModelDefined())
checkCS(self, cs, quasistatic=True, effector=True, wholeBody=True)
def store_mcapi_traj(self, traj_name):
if not mcapi_import:
print(
'multicontact_api package import has failed, check your install'
)
return
self.set_data_lst_as_arrays()
# trajectory with only one ContactPhase (simpler to read/write)
# when feet not in contact, the force is exactly zero, that's the only diff
cs = ContactSequence()
cp = ContactPhase()
# assign trajectories :
t_arr = self.data_log['t']
cp.timeInitial = t_arr[0]
cp.timeFinal = t_arr[-1]
cp.duration = t_arr[-1] - t_arr[0]
# col number of trajectories should be the time traj size hence the transpose
cp.q_t = piecewise.FromPointsList(self.data_log['q'].T, t_arr)
cp.dq_t = piecewise.FromPointsList(self.data_log['v'].T, t_arr)
cp.ddq_t = piecewise.FromPointsList(self.data_log['dv'].T, t_arr)
cp.tau_t = piecewise.FromPointsList(self.data_log['tau'].T, t_arr)
cp.c_t = piecewise.FromPointsList(self.data_log['c'].T, t_arr)
cp.dc_t = piecewise.FromPointsList(self.data_log['dc'].T, t_arr)
cp.L_t = piecewise.FromPointsList(self.data_log['Lc'].T, t_arr)
# contact force trajectories
for i_foot, frame_name in enumerate(self.contact_names):
cp.addContact(frame_name, ContactPatch(
pin.SE3(), 0.5)) # dummy placement and friction coeff
cp.addContactForceTrajectory(
frame_name,
piecewise.FromPointsList(self.data_log['f{}'.format(i_foot)].T,
t_arr))
cs.append(cp) # only one contact phase
savepath = os.path.join(self.directory, traj_name + '.cs')
cs.saveAsBinary(savepath)
print('Saved ' + savepath)
def compute_stopping_cs(self, move_to_support_polygon=True):
"""
Compute a Contact Sequence with centroidal trajectories to bring the current last_phase
to a stop without contact changes
:param move_to_support_polygon: if True, add a trajectory to put the CoM above the center of the support polygon
:return:
"""
phase_stop = ContactPhase(self.get_last_phase())
tools.setInitialFromFinalValues(phase_stop, phase_stop)
phase_stop.timeInitial = phase_stop.timeFinal
phase_stop.duration = DURATION_0_STEP # FIXME !!
# try 0-step:
success, phase = zeroStepCapturability(phase_stop, self.cfg)
if success:
cs_ref = ContactSequence(0)
cs_ref.append(phase)
# TEST : add another phase to go back in the center of the support polygon
if move_to_support_polygon:
phase_projected = ContactPhase()
phase_projected.timeInitial = phase.timeFinal
phase_projected.duration = DURATION_0_STEP
tools.copyContactPlacement(phase, phase_projected)
tools.setInitialFromFinalValues(phase, phase_projected)
phase_projected.c_final = tools.computeCenterOfSupportPolygonFromPhase(
phase_stop, self.fullBody.DEFAULT_COM_HEIGHT)
#FIXME 'default height'
tools.connectPhaseTrajToFinalState(phase_projected)
cs_ref.append(phase_projected)
else:
# TODO try 1 step :
raise RuntimeError("One step capturability not implemented yet !")
tools.computeRootTrajFromContacts(self.fullBody, cs_ref)
self.last_phase = cs_ref.contactPhases[-1].copy()
# define the final root position, translation from the CoM position and rotation from the feet rotation
q_final = np.zeros(7)
q_final[:3] = self.last_phase.c_final[::]
placement_rot_root, _ = tools.rootOrientationFromFeetPlacement(self.cfg.Robot, None, self.last_phase, None)
quat_root = Quaternion(placement_rot_root.rotation)
q_final[3:7] = [quat_root.x, quat_root.y, quat_root.z, quat_root.w]
self.last_phase.q_final = q_final
self.last_phase_flag.value = False
self.last_phase_pickled = Array(c_ubyte, MAX_PICKLE_SIZE) # reset currently stored whole body last phase
return cs_ref
def copyEffectorTrajectories(cs_eff, cs):
"""
Copy all the effector trajectories contained in cs_eff in a copy of cs
:param cs_eff: the contact sequence containing the effector trajectories
:param cs: the contact sequence to copy
:return: A new ContactSequence, which is a copy of cs with the addition of the trajectories from cs_eff
Return None if the phases do not have the same duration in cs_eff and cs
"""
if cs_eff.size() != cs.size():
return None
cs_res = ContactSequence(cs)
for pid, phase_eff in enumerate(cs_eff.contactPhases):
if len(phase_eff.effectorsWithTrajectory()) > 0:
phase = cs_res.contactPhases[pid]
if phase.timeInitial != phase_eff.timeInitial or phase.timeFinal != phase_eff.timeFinal:
print("Unable to copy effector trajectories, the phase duration have changed")
return None
for eeName, traj in phase_eff.effectorTrajectories().items():
phase.addEffectorTrajectory(eeName, traj)
return cs_res
def compute_centroidal(self, cs, previous_phase, last_iter=False):
"""
Solve the centroidal problem for the given ContactSequence
:param cs: the ContactSequence used
:param previous_phase: If provided, copy the final data of this phase as initial data for
the given ContactSequence
:param last_iter: If True, return a ContactSequence corresponding to the complete contactSequence given as input
If False, the result is splitted and only the first 3 phases are returned
:return: The ContactSequence with centroidal trajectories, and the last phase
"""
# update the initial state with the data from the previous intermediate state:
if previous_phase:
tools.setInitialFromFinalValues(previous_phase, cs.contactPhases[0])
self.cfg.COM_SHIFT_Z = 0.
self.cfg.TIME_SHIFT_COM = 0.
#else:
# self.cfg.COM_SHIFT_Z = self.previous_com_shift_z
# self.cfg.TIME_SHIFT_COM = self.previous_time_shift_com
if last_iter:
# Set settings specific to the last iteration that need to connect exactly to the final goal position
self.cfg.DURATION_CONNECT_GOAL = self.previous_connect_goal
self.cfg.TIMEOPT_CONFIG_FILE = self.TIMEOPT_CONFIG_FILE
else:
# Set settings for the middle of the sequence: do not need to connect exactly to the goal
self.cfg.DURATION_CONNECT_GOAL = 0.
self.cfg.TIMEOPT_CONFIG_FILE = self.TIMEOPT_CONFIG_FILE.rstrip(".yaml") + "_lowgoal.yaml"
if not self.CentroidalInputs.checkAndFillRequirements(cs, self.cfg, None):
raise RuntimeError(
"The current contact sequence cannot be given as input to the centroidal method selected.")
cs_full = self.generate_centroidal(self.cfg, cs, None, None)
# CentroidalOutputs.assertRequirements(cs_full)
if last_iter:
return cs_full, None
else:
cs_cut = ContactSequence(0)
for i in range(3):
cs_cut.append(cs_full.contactPhases[i])
return cs_cut, cs_cut.contactPhases[1]
def generateEffectorTrajectoriesForSequence(cs, fullBody=None):
"""
Generate an effector trajectory for each effectors which are going to be in contact in the next phase
:param cs:
:return:
"""
cs_res = ContactSequence(cs)
effectors = cs_res.getAllEffectorsInContact()
previous_phase = None
for pid in range(
cs_res.size() -
1): # -1 as last phase never have effector trajectories
phase = cs_res.contactPhases[pid]
next_phase = cs_res.contactPhases[pid + 1]
if pid > 0:
previous_phase = cs_res.contactPhases[pid - 1]
for eeName in effectors:
if not phase.isEffectorInContact(
eeName) and next_phase.isEffectorInContact(eeName):
# eeName will be in compute in the next phase, a trajectory should be added in the current phase
placement_end = next_phase.contactPatch(eeName).placement
time_interval = [phase.timeInitial, phase.timeFinal]
if previous_phase is not None and previous_phase.isEffectorInContact(
eeName):
placement_init = previous_phase.contactPatch(
eeName).placement
else:
placement_init = effectorPlacementFromPhaseConfig(
phase, eeName, fullBody)
# build the trajectory :
traj = generateEndEffectorTraj(time_interval,
placement_init,
placement_end)
phase.addEffectorTrajectory(eeName, traj)
return cs_res
from pinocchio import SE3
from numpy import array
from talos_rbprm.talos import Robot # change robot here
multicontact_api.switchToNumpyArray()
ENV_NAME = "multicontact/ground"
# Build the robot object and the viewer
fb, v = display_tools.initScene(Robot, ENV_NAME, False)
gui = v.client.gui
sceneName = v.sceneName
# display the origin and x,y,z axis in the viewer :
v.addLandmark(sceneName, 1)
#initialize an empty contact sequence
cs = ContactSequence(0)
# start from the reference configuration of the robot :
q_ref = fb.referenceConfig[::] + [0] * 6
#q_ref[0:2] = [ , ] # change the x,y position of the base as needed
# q_ref[2] += # the z position of the base already correspond to a floor at z=0, change it accordingly to the environment
# q_ref[3:7] = # quaternion (x,y,z) of the base
# display the configuration
v(q_ref)
com = array(fb.getCenterOfMass())
# create a first contact phase corresponding to this configuration
limbsInContact = [fb.rLegId, fb.lLegId
] # define the limbs in contact for this first phase
addPhaseFromConfig(fb, cs, q_ref, limbsInContact)
def CSfromMomentumopt(planner_setting,
cs,
init_state,
dyn_states,
t_init=0,
connect_goal=True):
"""
Create a ContactSequence and fill it with the results from momentumopt
:param planner_setting:
:param cs: a multicontact_api ContactSequence from which the contacts are copied
:param init_state: initial state used by momentumopt
:param dyn_states: the results of momentumopt
:return: a multicontact_api ContactSequence with centroidal trajectories
"""
logger.info("Start to convert result to mc-api ...")
cs_com = ContactSequence(cs)
MASS = planner_setting.get(mopt.PlannerDoubleParam_RobotMass)
p_id = 0 # phase id in cs
# dyn_states[0] is at t == dt , not t == 0 ! use init_state for t == 0
p0 = cs_com.contactPhases[0]
c_init = init_state.com
p0.timeInitial = t_init
# init arrays to store the discrete points. one value per columns
c_t = c_init.reshape(3, 1)
dc_t = p0.dc_init.reshape(3, 1)
ddc_t = p0.ddc_init.reshape(3, 1)
L_t = p0.L_init.reshape(3, 1)
dL_t = p0.dL_init.reshape(3, 1)
times = array(t_init)
current_t = t_init
# loop for all dynamicsStates
for k, ds in enumerate(dyn_states):
#extract states values from ds :
if k == 0:
ddc = (ds.lmom / MASS) / ds.dt # acceleration
dL = ds.amom / ds.dt # angular momentum variation
else:
ddc = ((ds.lmom / MASS) - (dyn_states[k - 1].lmom / MASS)) / ds.dt
dL = (ds.amom - dyn_states[k - 1].amom) / ds.dt
c = ds.com # position
dc = ds.lmom / MASS # velocity
L = ds.amom # angular momentum
# stack the values in the arrays:
c_t = append(c_t, c.reshape(3, 1), axis=1)
dc_t = append(dc_t, dc.reshape(3, 1), axis=1)
ddc_t = append(ddc_t, ddc.reshape(3, 1), axis=1)
L_t = append(L_t, L.reshape(3, 1), axis=1)
dL_t = append(dL_t, dL.reshape(3, 1), axis=1)
current_t += ds.dt
times = append(times, current_t)
if k > 0 and isNewPhase(dyn_states[k - 1],
ds) and p_id < cs_com.size() - 1:
#last k of current phase, first k of next one (same trajectories and time)
# set the trajectories for the current phase from the arrays :
phase = cs_com.contactPhases[p_id]
setCOMtrajectoryFromPoints(phase,
c_t,
dc_t,
ddc_t,
times,
overwriteInit=(p_id > 0))
setAMtrajectoryFromPoints(phase,
L_t,
dL_t,
times,
overwriteInit=(p_id > 0))
# set final time :
phase.timeFinal = times[-1]
# Start new phase :
p_id += 1
phase = cs_com.contactPhases[p_id]
# set initial time :
phase.timeInitial = times[-1]
# reset arrays of values to only the last point :
c_t = c_t[:, -1].reshape(3, 1)
dc_t = dc_t[:, -1].reshape(3, 1)
ddc_t = ddc_t[:, -1].reshape(3, 1)
L_t = L_t[:, -1].reshape(3, 1)
dL_t = dL_t[:, -1].reshape(3, 1)
times = times[-1]
# set final phase :
phase = cs_com.contactPhases[-1]
setCOMtrajectoryFromPoints(phase,
c_t,
dc_t,
ddc_t,
times,
overwriteFinal=not connect_goal)
setAMtrajectoryFromPoints(phase,
L_t,
dL_t,
times,
overwriteFinal=not connect_goal)
# set final time :
phase.timeFinal = times[-1]
logger.info("Converting results to mc-api done.")
return cs_com
def generate_centroidal_croc(cfg,
cs,
cs_initGuess=None,
fb=None,
viewer=None,
first_iter=True):
if cs_initGuess:
logger.warning("Initial guess is ignored.")
if not fb:
raise ValueError("CROC called without fullBody object.")
if not first_iter:
logger.warning("It is useless to iterate several times.")
beginId, endId = createFullbodyStatesFromCS(cs, fb)
logger.info("beginid = %d", beginId)
logger.info("endId = %d", endId)
cs_result = ContactSequence(cs)
resetCOMtrajectories(cs_result) # erase all pre existing CoM trajectories
if (2 * (endId - beginId) + 1) != cs.size():
raise NotImplemented(
"Current implementation of CROC require to have 2 contact phases per States in fullBody"
)
# for each phase in the cs, create a corresponding FullBody State and call CROC,
# then fill the cs struct
id_phase = 0
current_t = cs.contactPhases[0].timeInitial
for id_state in range(beginId, endId):
logger.info("id_state = %d", id_state)
logger.info("id_phase = %d", id_phase)
# First, compute the bezier curves between the states :
pid = fb.isDynamicallyReachableFromState(id_state,
id_state + 1,
True,
numPointsPerPhases=0)
if len(pid) != 4:
logger.error("# Cannot compute CROC between states %d, %d",
id_state, id_state + 1)
return cs
c1 = fb.getPathAsBezier(int(pid[1])) # curve before contact break
c2 = fb.getPathAsBezier(int(pid[2])) # curve for swing phase
c3 = fb.getPathAsBezier(int(pid[3])) # curve after contact creation
# NOTE : this bezier are defined in [0,tmax] ! We need to switch the time intervals
# fill the phases with the curves from CROC :
# c1 :
phase = cs_result.contactPhases[id_phase]
setCOMfromCurve(phase, c1)
current_t = phase.timeFinal
# c2
id_phase += 1
phase = cs_result.contactPhases[id_phase]
phase.timeInitial = current_t
setCOMfromCurve(phase, c2)
current_t = phase.timeFinal
# c3
id_phase += 1
phase = cs_result.contactPhases[id_phase]
phase.timeInitial = current_t
setCOMfromCurve(phase, c3)
# pid should not increase here, as the next c1 trajectory will be added to the same phase as the current c3 one.
return cs_result
def generate_wholebody_tsid(cfg, cs_ref, fullBody=None, viewer=None, robot=None, queue_qt = None):
"""
Generate the whole body motion corresponding to the given contactSequence
:param cs: Contact sequence containing the references,
it will only be modified if the end effector trajectories are not valid.
New references will be generated and added to the cs
:param fullBody: Required to compute collision free end effector trajectories
:param viewer: If provided, and the settings are enabled, display the end effector trajectories and the last step computed
:param robot: a tsid.RobotWrapper instance. If None, a new one is created from the urdf files defined in cfg
:param queue_qt: If not None, the joint trajectories are send to this multiprocessing.Queue during computation
The queue take a tuple: [q_t (a Curve object), ContactPhase (may be None), Bool (True mean that this is the
last trajectory of the motion)]
:return: a new ContactSequence object, containing the wholebody trajectories,
and the other trajectories computed from the wholebody motion request with cfg.IK_STORE_* and a robotWrapper instance
"""
# define nested functions used in control loop #
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 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 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 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 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 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 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 storeData(first_iter_for_phase = False):
"""
Append all the required data (selected in the configuration file) to the current ContactPhase
:param first_iter_for_phase: if True, set the initial values for the current phase
and correctly initiliaze empty trajectories for this phase
"""
if cfg.IK_store_joints_derivatives:
appendJointsDerivatives(first_iter_for_phase)
appendJointsValues(first_iter_for_phase)
if cfg.IK_store_joints_torque:
appendTorques(first_iter_for_phase)
if cfg.IK_store_centroidal:
appendCentroidal(first_iter_for_phase)
if cfg.IK_store_zmp:
appendZMP(first_iter_for_phase)
if cfg.IK_store_effector:
appendEffectorsTraj(first_iter_for_phase)
if cfg.IK_store_contact_forces:
appendContactForcesTrajs(first_iter_for_phase)
def printIntermediate():
"""
Print the current state: active contacts, tracking errors, computed joint acceleration and velocity
:return:
"""
if logger.isEnabledFor(logging.INFO):
print("Time %.3f" % (t))
for eeName, contact in dic_contacts.items():
if invdyn.checkContact(contact.name, sol):
f = invdyn.getContactForce(contact.name, sol)
print("\tnormal force %s: %.1f" % (contact.name.ljust(20, '.'), contact.getNormalForce(f)))
print("\ttracking err %s: %.3f" % (comTask.name.ljust(20, '.'), norm(comTask.position_error, 2)))
for eeName in phase.effectorsWithTrajectory():
task = dic_effectors_tasks[eeName]
error = task.position_error
if cfg.Robot.cType == "_3_DOF":
error = error[0:3]
print("\ttracking err %s: %.3f" % (task.name.ljust(20, '.'), norm(error, 2)))
print("\t||v||: %.3f\t ||dv||: %.3f" % (norm(v, 2), norm(dv)))
def checkDiverge():
"""
Check if either the joint velocity or acceleration is over a treshold or is NaN, and raise an error
"""
if norm(dv) > 1e6 or norm(v) > 1e6:
logger.error("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
logger.error("/!\ ABORT : controler unstable at t = %f /!\ ", t)
logger.error("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
raise ValueError("ABORT : controler unstable at t = " + str(t))
if math.isnan(norm(dv)) or math.isnan(norm(v)):
logger.error("!!!!!! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
logger.error("/!\ ABORT : nan at t = %f /!\ ", t)
logger.error("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
raise ValueError("ABORT : controler unstable at t = " + str(t))
def stopHere():
"""
Set the current data as final values for the current phase, resize the contact sequence if needed and return
:return: [The current ContactSequence, the RobotWrapper]
"""
setPreviousFinalValues(phase_prev, phase, cfg)
if cfg.WB_ABORT_WHEN_INVALID:
# cut the sequence up to the last phase
cs.resize(pid)
return cs, robot
elif cfg.WB_RETURN_INVALID:
# cut the sequence up to the current phase
cs.resize(pid+1)
return cs, robot
### End of nested functions definitions ###
if not viewer:
logger.warning("No viewer linked, cannot display end_effector trajectories.")
logger.warning("Start TSID ... ")
# copy the given contact sequence to keep it as reference :
cs = ContactSequence(cs_ref)
# delete all the 'reference' trajectories from result (to leave room for the real trajectories stored)
deleteAllTrajectories(cs)
# Create a robot wrapper
if robot is None or cfg.IK_store_centroidal or cfg.IK_store_zmp:
rp = RosPack()
package_name = cfg.Robot.packageName.split("/")[0]
package_path = rp.get_path(package_name)
urdf = package_path + cfg.Robot.packageName.lstrip(
package_name) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf'
logger.info("load robot : %s", urdf)
if robot is None:
logger.info("load robot : %s", urdf)
robot = tsid.RobotWrapper(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer(), False)
else:
logger.info("Use given robot in tsid.")
logger.info("robot loaded in tsid.")
if cfg.IK_store_centroidal or cfg.IK_store_zmp:
logger.info("load pinocchio robot ...")
# FIXME : tsid robotWrapper don't have all the required methods, only pinocchio have them
pinRobot = pin.RobotWrapper.BuildFromURDF(urdf, package_path, pin.JointModelFreeFlyer())
logger.info("pinocchio robot loaded.")
# get the selected end effector trajectory generation method
effectorMethod, effectorCanRetry = cfg.get_effector_method()
# get the selected simulator method
Simulator = cfg.get_simulator_class()
simulator = Simulator(cfg.IK_dt, robot.model())
### Define initial state of the robot ###
phase0 = cs.contactPhases[0]
q = phase0.q_init[:robot.nq].copy()
if not q.any():
raise RuntimeError("The contact sequence doesn't contain an initial whole body configuration")
t = phase0.timeInitial
if cs_ref.contactPhases[0].dq_t and cs_ref.contactPhases[0].dq_t.min() <= t <= cs_ref.contactPhases[0].dq_t.max():
v = cs_ref.contactPhases[0].dq_t(t)
else:
v = np.zeros(robot.nv)
logger.debug("V_init used in tsid : %s", v)
invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
invdyn.computeProblemData(t, q, v)
simulator.init(q, v)
# add initial contacts :
dic_contacts = {}
for eeName in cs.contactPhases[0].effectorsInContact():
# replace the initial contact patch placements if needed to match exactly the current position in the problem:
updateContactPlacement(cs, 0, eeName,
getCurrentEffectorPosition(robot, invdyn.data(), eeName),
cfg.Robot.cType == "_6_DOF")
# create the contacts :
contact = createContactForEffector(cfg, invdyn, robot, eeName, phase0.contactPatch(eeName), False)
dic_contacts.update({eeName: contact})
if cfg.EFF_CHECK_COLLISION: # initialise object needed to check the motion
from mlp.utils import check_path
validator = check_path.PathChecker(fullBody, cfg.CHECK_DT, logger.isEnabledFor(logging.INFO))
### Initialize all task used ###
logger.info("initialize tasks : ")
if cfg.w_com > 0. :
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(cfg.kp_com * np.ones(3))
comTask.setKd(2.0 * np.sqrt(cfg.kp_com) * np.ones(3))
invdyn.addMotionTask(comTask, cfg.w_com, cfg.level_com, 0.0)
else:
comTask = None
if cfg.w_am > 0.:
amTask = tsid.TaskAMEquality("task-am", robot)
amTask.setKp(cfg.kp_am * np.array([1., 1., 0.]))
amTask.setKd(2.0 * np.sqrt(cfg.kp_am * np.array([1., 1., 0.])))
invdyn.addMotionTask(amTask, cfg.w_am, cfg.level_am, 0.)
else:
amTask = None
if cfg.w_posture > 0.:
postureTask = tsid.TaskJointPosture("task-joint-posture", robot)
postureTask.setKp(cfg.kp_posture * cfg.gain_vector)
postureTask.setKd(2.0 * np.sqrt(cfg.kp_posture * cfg.gain_vector))
postureTask.setMask(cfg.masks_posture)
invdyn.addMotionTask(postureTask, cfg.w_posture, cfg.level_posture, 0.0)
q_ref = cfg.IK_REFERENCE_CONFIG
samplePosture = tsid.TrajectorySample(q_ref.shape[0] - 7)
samplePosture.pos(q_ref[7:]) # -7 because we remove the freeflyer part
else :
postureTask = None
if cfg.w_rootOrientation > 0. :
orientationRootTask = tsid.TaskSE3Equality("task-orientation-root", robot, 'root_joint')
mask = np.ones(6)
mask[0:3] = 0
mask[5] = cfg.YAW_ROT_GAIN
orientationRootTask.setMask(mask)
orientationRootTask.setKp(cfg.kp_rootOrientation * mask)
orientationRootTask.setKd(2.0 * np.sqrt(cfg.kp_rootOrientation * mask))
invdyn.addMotionTask(orientationRootTask, cfg.w_rootOrientation, cfg.level_rootOrientation, 0.0)
else:
orientationRootTask = None
# init effector task objects :
usedEffectors = cs.getAllEffectorsInContact()
dic_effectors_tasks = createEffectorTasksDic(cfg, usedEffectors, robot)
# Add bounds tasks if required:
if cfg.w_torque_bounds > 0.:
tau_max = cfg.scaling_torque_bounds*robot.model().effortLimit[-robot.na:]
tau_min = -tau_max
actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds", robot)
actuationBoundsTask.setBounds(tau_min, tau_max)
invdyn.addActuationTask(actuationBoundsTask, cfg.w_torque_bounds, 0, 0.0)
if cfg.w_joint_bounds > 0.:
jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot, cfg.IK_dt)
v_max = cfg.scaling_vel_bounds * robot.model().velocityLimit[-robot.na:]
v_min = -v_max
print("v_max : ", v_max)
jointBoundsTask.setVelocityBounds(v_min, v_max)
invdyn.addMotionTask(jointBoundsTask, cfg.w_joint_bounds, 0, 0.0)
solver = tsid.SolverHQuadProg("qp solver")
solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)
# time check
dt = cfg.IK_dt
logger.info("dt : %f", dt)
logger.info("tsid initialized, start control loop")
#raw_input("Enter to start the motion (motion displayed as it's computed, may be slower than real-time)")
time_start = time.time()
# For each phases, create the necessary task and references trajectories :
for pid in range(cs.size()):
logger.info("## for phase : %d", pid)
logger.info("t = %f", t)
# phase_ref contains the reference trajectories and should not be modified exept for new effector trajectories
# when the first ones was not collision free
phase_ref = cs_ref.contactPhases[pid]
# phase de not contains trajectories (exept for the end effectors) and should be modified with the new values computed
phase = cs.contactPhases[pid]
if pid > 0:
phase_prev = cs.contactPhases[pid - 1]
else:
phase_prev = None
if pid < cs.size() - 1:
phase_next = cs.contactPhases[pid + 1]
else:
phase_next = None
time_interval = [phase_ref.timeInitial, phase_ref.timeFinal]
logger.info("time_interval %s", time_interval)
# take CoM and AM trajectory from the phase, with their derivatives
com_traj = [phase_ref.c_t, phase_ref.dc_t, phase_ref.ddc_t]
am_traj = [phase_ref.L_t, phase_ref.L_t, phase_ref.dL_t]
# add root's orientation ref from reference config :
root_traj = phase_ref.root_t
# add se3 tasks for end effector when required
for eeName in phase.effectorsWithTrajectory():
logger.info("add se3 task for %s", eeName)
task = dic_effectors_tasks[eeName]
invdyn.addMotionTask(task, cfg.w_eff, cfg.level_eff, 0.)
adjustEndEffectorTrajectoryIfNeeded(cfg, phase_ref, robot, invdyn.data(), eeName, effectorMethod)
logger.info("t interval : %s", time_interval)
# add newly created contacts :
new_contacts_names = [] # will store the names of the contact tasks created at this phase
for eeName in usedEffectors:
if phase_prev and phase_ref.isEffectorInContact(eeName) and not phase_prev.isEffectorInContact(eeName):
invdyn.removeTask(dic_effectors_tasks[eeName].name, 0.0) # remove pin task for this contact
logger.info("remove se3 effector task : %s", dic_effectors_tasks[eeName].name)
if logger.isEnabledFor(logging.DEBUG):
current_placement = getCurrentEffectorPosition(robot, invdyn.data(), eeName)
logger.debug("Current effector placement : %s", current_placement)
logger.debug("Reference effector placement : %s", cs.contactPhases[pid].contactPatch(eeName).placement)
updateContactPlacement(cs, pid, eeName,
getCurrentEffectorPosition(robot, invdyn.data(), eeName),
cfg.Robot.cType == "_6_DOF")
contact = createContactForEffector(cfg, invdyn, robot, eeName, phase.contactPatch(eeName))
new_contacts_names += [contact.name]
dic_contacts.update({eeName: contact})
logger.info("Create contact for : %s", eeName)
# start removing the contact that will be broken in the next phase :
# (This tell the solver that it should start minimizing the contact force on this contact, and ideally get to 0 at the given time)
for eeName, contact in dic_contacts.items():
if phase_next and phase.isEffectorInContact(eeName) and not phase_next.isEffectorInContact(eeName):
transition_time = phase.duration + dt/2.
logger.info("\nTime %.3f Start breaking contact %s. transition time : %.3f\n",
t, contact.name, transition_time)
exist = invdyn.removeRigidContact(contact.name, transition_time)
assert exist, "Try to remove a non existing contact !"
# Remove all effectors not in contact at this phase,
# This is required as the block above may not remove the contact exactly at the desired time
# FIXME: why is it required ? Numerical approximation in the transition_time ?
for eeName, contact in dic_contacts.items():
if not phase.isEffectorInContact(eeName):
exist = invdyn.removeRigidContact(contact.name, 0.)
if exist:
logger.warning("Contact "+eeName+" was not remove after the given transition time.")
if cfg.WB_STOP_AT_EACH_PHASE:
input('start simulation')
# save values at the beginning of the current phase
q_begin = q.copy()
v_begin = v.copy()
phase.q_init = q_begin
if phase_prev:
phase_prev.q_final = q_begin
phaseValid = False
iter_for_phase = -1
# iterate until a valid motion for this phase is found (ie. collision free and which respect joint-limits)
while not phaseValid:
deletePhaseWBtrajectories(phase) # clean previous invalid trajectories
t = phase.timeInitial
k_t = 0
if iter_for_phase >= 0:
# reset values to their value at the beginning of the current phase
q = q_begin.copy()
v = v_begin.copy()
simulator.q = q
simulator.v = v
iter_for_phase += 1
logger.info("Start computation for phase %d , t = %f, try number : %d", pid, t, iter_for_phase)
# loop to generate states (q,v,a) for the current contact phase :
while t < phase.timeFinal - (dt / 2.):
# set traj reference for current time :
# com
if comTask:
sampleCom = curvesToTSID(com_traj,t)
comTask.setReference(sampleCom)
# am
if amTask:
if cfg.IK_trackAM:
sampleAM = curvesToTSID(am_traj,t)
else:
sampleAM = tsid.TrajectorySample(3)
amTask.setReference(sampleAM)
# posture
#print "postural task ref : ",samplePosture.pos()
if postureTask:
postureTask.setReference(samplePosture)
# root orientation :
if orientationRootTask:
sampleRoot = curveSE3toTSID(root_traj,t)
orientationRootTask.setReference(sampleRoot)
# update weight of regularization tasks for the new contacts:
if len(new_contacts_names) > 0 :
# linearly decrease the weight of the tasks for the newly created contacts
u_w_force = (t - phase.timeInitial) / (phase.duration * cfg.w_forceRef_time_ratio)
if u_w_force <= 1.:
current_w_force = cfg.w_forceRef_init * (1. - u_w_force) + cfg.w_forceRef_end * u_w_force
for contact_name in new_contacts_names:
success = invdyn.updateRigidContactWeights(contact_name, current_w_force)
assert success, "Unable to change the weight of the force regularization task for contact "+contact_name
logger.debug("### references given : ###")
logger.debug("com pos : %s", sampleCom.pos())
logger.debug("com vel : %s", sampleCom.vel())
logger.debug("com acc : %s", sampleCom.acc())
if amTask:
logger.debug("AM pos : %s", sampleAM.pos())
logger.debug("AM vel : %s", sampleAM.vel())
logger.debug("root pos : %s", sampleRoot.pos())
logger.debug("root vel : %s", sampleRoot.vel())
# end effector (if they exists)
for eeName, traj in phase_ref.effectorTrajectories().items():
sampleEff = curveSE3toTSID(traj,t,True)
dic_effectors_tasks[eeName].setReference(sampleEff)
logger.debug("effector %s, pos = %s", eeName, sampleEff.pos())
logger.debug("effector %s, vel = %s", eeName, sampleEff.vel())
logger.debug("previous q = %s", q)
logger.debug("previous v = %s", v)
# solve HQP for the current time
HQPData = invdyn.computeProblemData(t, q, v)
if t < phase.timeInitial + dt:
logger.info("final data for phase %d", pid)
if logger.isEnabledFor(logging.INFO):
HQPData.print_all()
sol = solver.solve(HQPData)
dv = invdyn.getAccelerations(sol)
storeData(k_t == 0)
q, v = simulator.simulate(dv)
t += dt
k_t += 1
if t >= phase.timeFinal - (dt / 2.):
t = phase.timeFinal # avoid numerical imprecisions
logger.debug("v = %s", v)
logger.debug("dv = %s", dv)
if int(t / dt) % cfg.IK_PRINT_N == 0:
printIntermediate()
try:
checkDiverge()
except ValueError:
return stopHere()
# end while t \in phase_t (loop for the current contact phase)
if len(phase.effectorsWithTrajectory()) > 0 and cfg.EFF_CHECK_COLLISION:
phaseValid, t_invalid = validator.check_motion(phase.q_t)
#if iter_for_phase < 3 :# debug only, force limb-rrt
# phaseValid = False
# t_invalid = (phase.timeInitial + phase.timeFinal) / 2.
if not phaseValid:
if iter_for_phase == 0:
# save the first q_t trajectory computed, for limb-rrt
first_q_t = phase.q_t
logger.warning("Phase %d not valid at t = %f", pid, t_invalid)
logger.info("First invalid q : %s", phase.q_t(t_invalid))
if t_invalid <= (phase.timeInitial + cfg.EFF_T_PREDEF) \
or t_invalid >= (phase.timeFinal - cfg.EFF_T_PREDEF):
logger.error("Motion is invalid during predefined phases, cannot change this.")
return stopHere()
if effectorCanRetry():
logger.warning("Try new end effector trajectory.")
try:
for eeName, ref_traj in phase_ref.effectorTrajectories().items():
placement_init = ref_traj.evaluateAsSE3(phase.timeInitial)
placement_end = ref_traj.evaluateAsSE3(phase.timeFinal)
traj = effectorMethod(cfg, time_interval, placement_init, placement_end,
iter_for_phase + 1, first_q_t, phase_prev,
phase_ref, phase_next, fullBody, eeName, viewer)
# save the new trajectory in the phase with the references
phase_ref.addEffectorTrajectory(eeName,traj)
if cfg.DISPLAY_ALL_FEET_TRAJ and logger.isEnabledFor(logging.INFO):
color = fullBody.dict_limb_color_traj[eeName]
color[-1] = 0.6 # set small transparency
display_tools.displaySE3Traj(phase_ref.effectorTrajectory(eeName),
viewer.client.gui,
viewer.sceneName,
eeName + "_traj_" + str(pid) + "_" + str(iter_for_phase),
color,
[phase.timeInitial, phase.timeFinal],
fullBody.dict_offset[eeName])
except ValueError as e:
logging.error("ERROR in generateEndEffectorTraj :", exc_info=e)
return stopHere()
else:
logging.error("End effector method choosen do not allow retries, abort here.")
return stopHere()
else: # no effector motions, phase always valid (or bypass the check)
phaseValid = True
logging.info("Phase %d valid.", pid)
if phaseValid:
setPreviousFinalValues(phase_prev, phase, cfg)
# display the progress by moving the robot at the last configuration computed
if viewer and cfg.IK_SHOW_PROGRESS:
display_tools.displayWBconfig(viewer,q)
#end while not phaseValid
# end for all phases
# store the data of the last point
phase_prev = phase
phase = ContactPhase(phase_prev)
storeData(True)
setPreviousFinalValues(phase_prev, phase, cfg)
time_end = time.time() - time_start
logger.warning("Whole body motion generated in : %f s.", time_end)
logger.info("\nFinal COM Position %s", robot.com(invdyn.data()))
logger.info("Desired COM Position %s", cs.contactPhases[-1].c_final)
if queue_qt:
queue_qt.put([phase.q_t.curve_at_index(phase.q_t.num_curves() - 1), None, True])
return cs, robot
def build_cs_from_sl1m_mip(fb, q_ref, root_end, pb, allfeetpos, use_orientation, use_interpolated_orientation,
q_init = None, first_phase = None):
"""
Build a multicontact_api.ContactSequence from the SL1M outputs when using the MIP formulation
:param fb: an instance of rbprm.Fullbody
:param q_ref: the reference wholebody configuration of the robot
:param root_end: the final base position
:param pb: the SL1M problem dictionary, containing all the contact surfaces and data
:param allfeetpos: the list of all foot position for each phase, computed by SL1M
:param use_orientation: if True, change the contact yaw rotation to match the orientation of the base in the guide
:param use_interpolated_orientation: if True, the feet yaw orientation will 'anticipate' the base orientation
of the next phase
:param q_init: the initial wholebody configuration (either this or first_phase should be provided)
:param first_phase: the first multicontact_api.ContactPhase object (either this or q_init should be provided)
:return: the multicontact_api.ContactSequence, with all ContactPhase created at the correct placement
"""
# init contact sequence with first phase : q_ref move at the right root pose and with both feet in contact
# FIXME : allow to customize that first phase
num_steps = len(pb["phaseData"]) - 1 # number of contact repositionning
limbs_names = fb.limbs_names
num_effectors = len(limbs_names)
logger.info(" limbs names : %s", limbs_names)
cs = ContactSequence(0)
if first_phase:
cs.append(first_phase)
elif q_init:
# if only the configuration is provided, assume all effectors are in contact
addPhaseFromConfig(fb, cs, q_init, limbs_names)
else:
raise ValueError("build_cs_from_sl1m should have either q_init or first_phase argument defined")
logger.info("Initial phase added, contacts : %s ", cs.contactPhases[0].effectorsInContact())
# loop for all effector placements, and create the required contact phases
previous_eff_placements = allfeetpos[0]
if len(previous_eff_placements) != num_effectors:
raise NotImplementedError("A phase in the output of SL1M do not have all the effectors in contact.")
for pid, eff_placements in enumerate(allfeetpos[1:]):
logger.info("Loop allfeetpos, id = %d", pid)
if len(eff_placements) != num_effectors:
raise NotImplementedError("A phase in the output of SL1M do not have all the effectors in contact.")
switch = False # True if a repostionning have been detected
for k, pos in enumerate(eff_placements):
if norm(pos - previous_eff_placements[k]) > 1e-3:
if switch:
raise NotImplementedError("Several contact changes between two adjacent phases in SL1M output")
switch = True
ee_name = fb.dict_limb_joint[limbs_names[k]]
#pos[2] += EPS_Z # FIXME: apply epsz along the normal
pos = fb.dict_offset[ee_name].actInv(pos)
logger.info("Move effector %s ", ee_name)
logger.info("To position %s ", pos)
placement = SE3.Identity()
placement.translation = pos
# compute orientation of the contact from the surface normal:
phase_data = pb["phaseData"][pid+1] # +1 because the for loop start at id = 1
n = normal_from_ineq(phase_data["S"][phase_data["id_surface"]])
placement.rotation = rotationFromNormal(n)
logger.debug("new contact placement : %s", placement)
# TODO add yaw rotation from guide here !
cs.moveEffectorToPlacement(ee_name, placement)
#if not switch:
# raise RuntimeError("No contact changes between two adjacent phases in SL1M output")
# assign com position to the last two phases :
# swinging phase, same init and final position
"""
cs.contactPhases[-2].c_init = coms[pid * 2]
cs.contactPhases[-2].c_final = coms[pid * 2 + 1]
# phase with all the contacts:
cs.contactPhases[-1].c_init = coms[pid * 2 + 1]
if pid * 2 + 2 < len(coms):
cs.contactPhases[-1].c_final = coms[pid * 2 + 2]
else:
cs.contactPhases[-1].c_final = cs.contactPhases[-1].c_init
"""
previous_eff_placements = eff_placements
# final phase :
# fixme : assume root is in the middle of the last 2 feet pos ...
"""
q_end = q_ref.tolist() + [0] * 6
# p_end = (allfeetpos[-1] + allfeetpos[-2]) / 2.
# for i in range(3):
# q_end[i] += p_end[i]
q_end[0:7] = root_end
feet_height_end = allfeetpos[-1][0][2]
logger.info("feet height final = %s", feet_height_end)
q_end[2] = feet_height_end + q_ref[2]
#q_end[2] += EPS_Z
fb.setCurrentConfig(q_end)
com = fb.getCenterOfMass()
setFinalState(cs, com, q=q_end)
"""
p_final = cs.contactPhases[-1]
p_final.c_final = computeCenterOfSupportPolygonFromPhase(p_final, fb.DEFAULT_COM_HEIGHT)
p_final.c_init = p_final.c_final
return cs
def build_cs_from_sl1m_l1(fb, q_ref, root_end, pb, RF, allfeetpos, use_orientation, use_interpolated_orientation,
q_init = None, first_phase = None):
"""
Build a multicontact_api.ContactSequence from the SL1M outputs, when not using the MIP formulation
:param fb: an instance of rbprm.Fullbody
:param q_ref: the reference wholebody configuration of the robot
:param root_end: the final base position
:param pb: the SL1M problem dictionary, containing all the contact surfaces and data
:param RF: the Id of the right feet in the SL1M formulation
:param allfeetpos: the list of all foot position for each phase, computed by SL1M
:param use_orientation: if True, change the contact yaw rotation to match the orientation of the base in the guide
:param use_interpolated_orientation: if True, the feet yaw orientation will 'anticipate' the base orientation
of the next phase
:param q_init: the initial wholebody configuration (either this or first_phase should be provided)
:param first_phase: the first multicontact_api.ContactPhase object (either this or q_init should be provided)
:return: the multicontact_api.ContactSequence, with all ContactPhase created at the correct placement
"""
# init contact sequence with first phase : q_ref move at the right root pose and with both feet in contact
# FIXME : allow to customize that first phase
cs = ContactSequence(0)
if q_init:
addPhaseFromConfig(fb, cs, q_init, [fb.rLegId, fb.lLegId])
elif first_phase:
cs.append(first_phase)
else:
raise ValueError("build_cs_from_sl1m should have either q_init or first_phase argument defined")
# loop over all phases of pb and add them to the cs :
for pId in range(2, len(pb["phaseData"])): # start at 2 because the first two ones are already done in the q_init
prev_contactPhase = cs.contactPhases[-1]
#n = normal(pb["phaseData"][pId])
phase = pb["phaseData"][pId]
moving = phase["moving"]
movingID = fb.lfoot
if moving == RF:
movingID = fb.rfoot
pos = allfeetpos[pId] # array, desired position for the feet movingID
pos[2] += EPS_Z # FIXME it shouldn't be required !!
# compute desired foot rotation :
if use_orientation:
rot = compute_orientation_for_feet_placement(fb, pb, pId, moving, RF, prev_contactPhase, use_interpolated_orientation)
else:
rot = Quaternion.Identity()
placement = SE3()
placement.translation = np.array(pos).T
placement.rotation = rot.matrix()
cs.moveEffectorToPlacement(movingID, placement)
# final phase :
# fixme : assume root is in the middle of the last 2 feet pos ...
q_end = q_ref.tolist() + [0] * 6
#p_end = (allfeetpos[-1] + allfeetpos[-2]) / 2.
#for i in range(3):
# q_end[i] += p_end[i]
q_end[0:7] = root_end
feet_height_end = allfeetpos[-1][2]
logger.info("feet height final = %s", feet_height_end)
q_end[2] = feet_height_end + q_ref[2]
q_end[2] += EPS_Z
fb.setCurrentConfig(q_end)
com = fb.getCenterOfMass()
setFinalState(cs, com, q=q_end)
return cs