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)
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)
transform = SE3.Identity()
cs.moveEffectorOf(fb.rfoot, transform)
cs.moveEffectorOf(fb.lfoot, transform)
cs.moveEffectorOf(fb.rfoot, transform)
cs.moveEffectorOf(fb.lfoot, transform)
setFinalState(cs, com, q_ref)
assert cs.haveConsistentContacts()
cs.saveAsBinary("talos_flatGround.cs")
cs.contactPhases[-2].c_init = c_r
cs.contactPhases[-2].c_final = c_r
cs.contactPhases[-1].c_init = c_r
cs.moveEffectorOf(fb.rfoot, transform)
cs.contactPhases[-3].c_final = c_l
cs.contactPhases[-2].c_init = c_l
cs.contactPhases[-2].c_final = c_l
cs.contactPhases[-1].c_init = c_l
cs.moveEffectorOf(fb.lfoot, transform)
cs.contactPhases[-3].c_final = c_r
cs.contactPhases[-2].c_init = c_r
cs.contactPhases[-2].c_final = c_r
cs.contactPhases[-1].c_init = c_r
setFinalState(cs, com, q_ref)
cs.saveAsBinary("step_in_place_quasistatic.cs")
t = 0
for pid, phase in enumerate(cs.contactPhases):
# define timing :
phase.timeInitial = t
if phase.numContacts() == 2:
phase.timeFinal = t + 2. # DS duration
else:
phase.timeFinal = t + 1.5 ## SS duration
t = phase.timeFinal
# define init/end CoM position :
if phase.numContacts() == 1:
phase.c_init = cs.contactPhases[pid - 1].c_final
phase.c_final = cs.contactPhases[pid - 1].c_final
else:
from mlp.utils.cs_tools import addPhaseFromConfig
import multicontact_api
from multicontact_api import ContactSequence
import mlp.viewer.display_tools as display_tools
from pinocchio import SE3
from numpy import array
from mlp.utils.cs_tools import walk
from solo_rbprm.solo import Robot # change robot here
multicontact_api.switchToNumpyArray()
ENV_NAME = "multicontact/ground"
fb, v = display_tools.initScene(Robot, ENV_NAME, False)
gui = v.client.gui
sceneName = v.sceneName
cs = ContactSequence(0)
limbs = fb.limbs_names
#Create an initial contact phase :
q_ref = fb.referenceConfig[::] + [0] * 6
addPhaseFromConfig(fb, cs, q_ref, limbs)
walk(fb, cs, 0.5, 0.1, limbs, first_half_step=False)
display_tools.displaySteppingStones(cs, gui, sceneName, fb)
filename = "solo_flatGround.cs"
print("Write contact sequence binary file : ", filename)
cs.saveAsBinary(filename)
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()
assert cs.haveConsistentContacts()
assert cs.haveCentroidalValues()
assert cs.haveCentroidalTrajectories()
cs.saveAsBinary("com_motion_above_feet_COM.cs")
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)
transform = SE3.Identity()
cs.moveEffectorOf(fb.rfoot, transform)
cs.moveEffectorOf(fb.lfoot, transform)
cs.moveEffectorOf(fb.rfoot, transform)
cs.moveEffectorOf(fb.lfoot, transform)
setFinalState(cs, com, q_ref)
assert cs.haveConsistentContacts()
cs.saveAsBinary("step_in_place.cs")