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)