def setJointsTrajectoryFromPoints(phase, q, dq, ddq, timeline, overwrite=True):
"""
Define the joints position, velocity and acceleration trajectories as a linear interpolation between each points
Also set the initial / final values for q, dq and ddq to match the ones in the trajectory
:param phase:
:param q:
:param dq:
:param ddq:
:param timeline:
:param overwrite: Default True : overwrite init/final values even if they exist
:return:
"""
phase.q_t = piecewise.FromPointsList(q, timeline.T)
phase.dq_t = piecewise.FromPointsList(dq, timeline.T)
phase.ddq_t = piecewise.FromPointsList(ddq, timeline.T)
if overwrite or not phase.q_init.any():
phase.q_init = q[:, 0]
if overwrite or not phase.dq_init.any():
phase.dq_init = dq[:, 0]
if overwrite or not phase.ddq_init.any():
phase.ddq_init = ddq[:, 0]
if overwrite or not phase.q_final.any():
phase.q_final = q[:, -1]
if overwrite or not phase.dq_final.any():
phase.dq_final = dq[:, -1]
if overwrite or not phase.ddq_final.any():
phase.ddq_final = ddq[:, -1]
def setCOMtrajectoryFromPoints(phase,
c,
dc,
ddc,
timeline,
overwriteInit=True,
overwriteFinal=True):
"""
Define the CoM position, velocity and acceleration trajectories as a linear interpolation between each points
Also set the initial / final values for c, dc and ddc to match the ones in the trajectory
:param phase:
:param c:
:param dc:
:param ddc:
:param timeline:
:param overwrite: Default True : overwrite init/final values even if they exist
:return:
"""
phase.c_t = piecewise.FromPointsList(c, timeline.T)
phase.dc_t = piecewise.FromPointsList(dc, timeline.T)
phase.ddc_t = piecewise.FromPointsList(ddc, timeline.T)
if overwriteInit:
phase.c_init = c[:, 0]
if overwriteInit:
phase.dc_init = dc[:, 0]
if overwriteInit:
phase.ddc_init = ddc[:, 0]
if overwriteFinal:
phase.c_final = c[:, -1]
if overwriteFinal:
phase.dc_final = dc[:, -1]
if overwriteFinal:
phase.ddc_final = ddc[:, -1]
def setAMtrajectoryFromPoints(phase,
L,
dL,
timeline,
overwriteInit=True,
overwriteFinal=True):
"""
Define the AM value and it's time derivative trajectories as a linear interpolation between each points
Also set the initial / final values for L and dL to match the ones in the trajectory
:param phase:
:param L:
:param dL:
:param timeline:
:param overwrite: Default True : overwrite init/final values even if they exist
:return:
"""
phase.L_t = piecewise.FromPointsList(L, timeline.T)
phase.dL_t = piecewise.FromPointsList(dL, timeline.T)
if overwriteInit:
phase.L_init = L[:, 0]
if overwriteInit:
phase.dL_init = dL[:, 0]
if overwriteFinal:
phase.L_final = L[:, -1]
if overwriteFinal:
phase.dL_final = dL[:, -1]
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)