def createPhaseFromConfig(fb, q, limbsInContact, t_init = -1):
"""
Build a contact phase from a wholebody configuration and a list of active contacts.
Set the q_init, c_init and c_final from the joint positions,
and add ContactPatch for each limb in contact, with the current placement corresponding to the wholebody configuration
:param fb: an rbprm.Fullbody object
:param q: a list of joint position
:param limbsInContact: a list of limbs currently in contact
:param t_init: if provided, the new COntactPhase will begin at this time
:return: a new ContactPhase
"""
phase = ContactPhase()
phase.q_init = np.array(q)
fb.setCurrentConfig(q)
com = np.array(fb.getCenterOfMass())
if t_init >= 0:
phase.timeInitial = t_init
phase.c_init = com.copy()
phase.c_final = com.copy()
if fb.client.robot.getDimensionExtraConfigSpace() >= 6 and len(q) == fb.getConfigSize():
# add dc and ddc values from extraDOF
phase.dc_init = np.array(q[-6:-3])
phase.dc_final = np.array(q[-6:-3])
phase.ddc_init = np.array(q[-3:])
phase.ddc_final = np.array(q[-3:])
for limb in limbsInContact:
eeName = fb.dict_limb_joint[limb]
q_j = fb.getJointPosition(eeName)
placement = SE3FromConfig(q_j)
if fb.cType == '_3_DOF':
placement.rotation = np.identity(3) # FIXME: use contact normal instead of identity, but it's unknown here
patch = ContactPatch(placement) # TODO set friction / other parameters here
phase.addContact(eeName, patch)
return phase
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 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 createPhaseFromConfig(fb, q, limbsInContact, t_init=-1):
phase = ContactPhase()
phase.q_init = np.array(q)
fb.setCurrentConfig(q)
com = np.array(fb.getCenterOfMass())
if t_init > 0:
phase.timeInitial = 0.
phase.c_init = com.copy()
phase.c_final = com.copy()
if fb.client.robot.getDimensionExtraConfigSpace() >= 6 and len(
q) == fb.getConfigSize():
# add dc and ddc values from extraDOF
phase.dc_init = np.array(q[-6:-3])
phase.dc_final = np.array(q[-6:-3])
phase.ddc_init = np.array(q[-3:])
phase.ddc_final = np.array(q[-3:])
for limb in limbsInContact:
eeName = fb.dict_limb_joint[limb]
q_j = fb.getJointPosition(eeName)
placement = SE3FromConfig(q_j)
patch = ContactPatch(
placement) # TODO set friction / other parameters here
phase.addContact(eeName, patch)
return phase
q_ref[24] -= 1.3 # lift up arms q_ref[32] -= 1.3 v(q_ref) addPhaseFromConfig(fb, cs, q_ref, [fb.rLegId, fb.lLegId]) pRH = SE3.Identity() pRH = rotatePlacement(pRH, 'y', -0.8115781021773631) pLH = pRH.copy() #pLH.translation = np.matrix([-2.635,-1.235,1.13]).T #pRH.translation = np.matrix([-2.635,-2.28,1.13]).T pLH.translation = array([-2.842, -1.235, 0.91]) pRH.translation = array([-2.842, -2.28, 0.91]) cs.createContact(fb.rhand, ContactPatch(pRH)) cs.createContact(fb.lhand, ContactPatch(pLH)) displacement = SE3.Identity() displacement.translation = array([0.15, 0, 0]) cs.moveEffectorOf(fb.rfoot, displacement) cs.moveEffectorOf(fb.lfoot, displacement) num_steps = 1 step_height = 0.22 step_width = 0.207 gait = [fb.rhand, fb.lhand, fb.rfoot, fb.lfoot] displacement.translation = array([step_width, 0, step_height]) for i in range(num_steps):
print("number of contact phases in the contact sequence : ", cs.size())
# there is now one phase in our 'cs' object
# we can now add new contact phases by changing the contacts of the last phase of the sequence:
cs.breakContact(
fb.rfoot
) # add a new contact phase to the sequence: break the right feet contact
# create a new contact for the right feet at a different position :
placement = cs.contactPhases[0].contactPatch(
fb.rfoot).placement.copy() # take the previous position of the right feet
pos = placement.translation
pos[0] += 0.15 # move of 15cm in the x direction
placement.translation = pos
# add a new contact phase, with a contact created for the right feet at the given placement :
cs.createContact(fb.rfoot, ContactPatch(placement))
#display the contacts in gepetto viewer :
#display_tools.displaySteppingStones(cs, gui, v.sceneName, fb)
print("number of contact phases in the contact sequence : ", cs.size())
# There is now 3 contact phases (corresponding to one step)
#first phase:
print("Right feet contact of phase 0 ")
print(cs.contactPhases[0].isEffectorInContact(fb.rfoot))
print(cs.contactPhases[0].contactPatch(fb.rfoot).placement)
print("Left feet contact of phase 0 ")
print(cs.contactPhases[0].isEffectorInContact(fb.lfoot))
print(cs.contactPhases[0].contactPatch(fb.lfoot).placement)
#second phase:
print("Right feet contact of phase 1 ")