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 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