def simulateTrajectory(config, trajectory, model=None, measurements=None): # generate data arrays for simulation and regressor building old_sim = config['simulateTorques'] config['simulateTorques'] = True if not model: model = Model(config, config['model']) data = Data(config) trajectory_data = {} trajectory_data['target_positions'] = [] trajectory_data['target_velocities'] = [] trajectory_data['target_accelerations'] = [] trajectory_data['torques'] = [] trajectory_data['times'] = [] freq=200.0 for t in range(0, int(trajectory.getPeriodLength()*freq)): trajectory.setTime(t/freq) q = [trajectory.getAngle(d) for d in range(config['N_DOFS'])] q = np.array(q) if config['useDeg']: q = np.deg2rad(q) trajectory_data['target_positions'].append(q) qdot = [trajectory.getVelocity(d) for d in range(config['N_DOFS'])] qdot = np.array(qdot) if config['useDeg']: qdot = np.deg2rad(qdot) trajectory_data['target_velocities'].append(qdot) qddot = [trajectory.getAcceleration(d) for d in range(config['N_DOFS'])] qddot = np.array(qddot) if config['useDeg']: qddot = np.deg2rad(qddot) trajectory_data['target_accelerations'].append(qddot) trajectory_data['times'].append(t/freq) trajectory_data['torques'].append(np.zeros(config['N_DOFS'])) num_samples = len(trajectory_data['times']) trajectory_data['target_positions'] = np.array(trajectory_data['target_positions']) trajectory_data['positions'] = trajectory_data['target_positions'] trajectory_data['target_velocities'] = np.array(trajectory_data['target_velocities']) trajectory_data['velocities'] = trajectory_data['target_velocities'] trajectory_data['target_accelerations'] = np.array(trajectory_data['target_accelerations']) trajectory_data['accelerations'] = trajectory_data['target_accelerations'] trajectory_data['torques'] = np.array(trajectory_data['torques']) trajectory_data['times'] = np.array(trajectory_data['times']) trajectory_data['measured_frequency'] = freq trajectory_data['base_velocity'] = np.zeros( (num_samples, 6) ) trajectory_data['base_acceleration'] = np.zeros( (num_samples, 6) ) trajectory_data['base_rpy'] = np.zeros( (num_samples, 3) ) trajectory_data['contacts'] = np.array({'dummy_sim': np.zeros( num_samples )}) if measurements: trajectory_data['positions'] = measurements['Q'] trajectory_data['velocities'] = measurements['V'] trajectory_data['accelerations'] = measurements['Vdot'] trajectory_data['measured_frequency'] = measurements['measured_frequency'] old_skip = config['skip_samples'] config['skip_samples'] = 0 old_offset = config['start_offset'] config['start_offset'] = 0 data.init_from_data(trajectory_data) model.computeRegressors(data) #TODO: this needlessly also computes regressors in addition to simulation config['skip_samples'] = old_skip config['start_offset'] = old_offset config['simulateTorques'] = old_sim return trajectory_data, data, model
Vdot=out['accelerations'], Tau=out['torques'], Tau_raw=out['torques_raw'], T=out['times'], Fs=out['frequency']) #simulate with iDynTree if we're using gazebo data if is_gazebo: #use all data old_skip = config['skipSamples'] config['skipSamples'] = 0 old_offset = config['startOffset'] config['startOffset'] = 0 old_sim = config['simulateTorques'] config['simulateTorques'] = 1 data.init_from_data(out) model = Model(config, config['model'], args.regressor) if config['verbose']: print('simulating torques for motion data') model.computeRegressors(data) out['torques'] = out['torques_raw'] = model.data.samples['torques'] config['skipSamples'] = old_skip config['startOffset'] = old_offset config['simulateTorques'] = old_sim if config['floatingBase']: np.savez(args.outfile, positions=out['positions'], positions_raw=out['positions'], target_positions=out['target_positions'], velocities=out['velocities'],
out['base_velocity'] = np.hstack( (out['IMUlinVel'], out['IMUrotVel']) ) out['base_acceleration'] = np.hstack( (out['IMUlinAcc'], out['IMUrotAcc']) ) out['contacts'] = np.array({'r_leg_ft': out['FTright']}) out['base_rpy'] = out['IMUrpy'] #simulate with iDynTree if we're using gazebo data if not is_hw: #use all data old_skip = config['skip_samples'] config['skip_samples'] = 0 old_offset = config['start_offset'] config['start_offset'] = 0 old_sim = config['simulateTorques'] config['simulateTorques'] = 1 data.init_from_data(out) model = Model(config, config['model']) model.computeRegressors(data) out['torques'] = out['torques_raw'] = model.data.samples['torques'] config['skip_samples'] = old_skip config['start_offset'] = old_offset config['simulateTorques'] = old_sim np.savez(args.outfile, positions=out['positions'], positions_raw=out['positions'], velocities=out['velocities'], velocities_raw=out['velocities'], accelerations=out['accelerations'], torques=out['torques'], torques_raw=out['torques_raw'], base_velocity=out['base_velocity'], base_acceleration=out['base_acceleration'], base_rpy=out['base_rpy'], contacts=out['contacts'], times=out['times'], frequency=out['frequency']) print("Saved csv data as {}".format(args.outfile)) print("Samples: {}, Time: {}s, Frequency: {} Hz".format(out['times'].shape[0], out['times'][-1], out['frequency']))
def simulateTrajectory(config, trajectory, model=None, measurements=None): # type: (Dict, Trajectory, Model, np._ArrayLike) -> Tuple[Dict, Data] # generate data arrays for simulation and regressor building old_sim = config['simulateTorques'] config['simulateTorques'] = True if config['floatingBase']: fb = 6 else: fb = 0 if not model: if 'urdf_real' in config and config['urdf_real']: print('Simulating using "real" model parameters.') urdf = config['urdf_real'] else: urdf = config['urdf'] model = Model(config, urdf) data = Data(config) trajectory_data = {} # type: Dict[str, Union[List, np._ArrayLike]] trajectory_data['target_positions'] = [] trajectory_data['target_velocities'] = [] trajectory_data['target_accelerations'] = [] trajectory_data['torques'] = [] trajectory_data['times'] = [] freq = config['excitationFrequency'] #f= open("trajectory3.txt","a+") for t in range(0, int(trajectory.getPeriodLength() * freq)): trajectory.setTime(t / freq) q = np.array( [trajectory.getAngle(d) for d in range(config['num_dofs'])]) if config['useDeg']: q = np.deg2rad(q) trajectory_data['target_positions'].append(q) #f.write("%s\n" % q) qdot = np.array( [trajectory.getVelocity(d) for d in range(config['num_dofs'])]) if config['useDeg']: qdot = np.deg2rad(qdot) trajectory_data['target_velocities'].append(qdot) qddot = np.array( [trajectory.getAcceleration(d) for d in range(config['num_dofs'])]) if config['useDeg']: qddot = np.deg2rad(qddot) trajectory_data['target_accelerations'].append(qddot) trajectory_data['times'].append(t / freq) trajectory_data['torques'].append(np.zeros(config['num_dofs'] + fb)) #f.write("next iteration \n") #f.close() num_samples = len(trajectory_data['times']) #convert lists to numpy arrays trajectory_data['target_positions'] = np.array( trajectory_data['target_positions']) trajectory_data['positions'] = trajectory_data['target_positions'] trajectory_data['target_velocities'] = np.array( trajectory_data['target_velocities']) trajectory_data['velocities'] = trajectory_data['target_velocities'] trajectory_data['target_accelerations'] = np.array( trajectory_data['target_accelerations']) trajectory_data['accelerations'] = trajectory_data['target_accelerations'] trajectory_data['torques'] = np.array(trajectory_data['torques']) trajectory_data['times'] = np.array(trajectory_data['times']) trajectory_data['measured_frequency'] = freq trajectory_data['base_velocity'] = np.zeros((num_samples, 6)) trajectory_data['base_acceleration'] = np.zeros((num_samples, 6)) trajectory_data['base_rpy'] = np.zeros((num_samples, 3)) # add static contact force contacts = 1 if config['floatingBase'] and contacts: contactFrame = 'contact_ft' # get base acceleration that results from acceleration at contact frame #contact_wrench = np.zeros(6) #contact_wrench[2] = 9.81 * 3.0 # -g * mass contact_wrench = np.random.rand(6) * 10 trajectory_data['base_rpy'] = np.random.rand( (3 * num_samples)).reshape((num_samples, 3)) * 0.5 """ contact_wrench[2] = 9.81 # * 139.122814 len_contact = la.norm(contact_wrench[0:3]) # get vector from contact frame to robot center of mass model_com = iDynTree.Position() model_com = model.dynComp.getWorldTransform(contactFrame).inverse()*model.dynComp.getCenterOfMass() model_com = model_com.toNumPy() # rotate contact wrench to be in line with COM (so that base link is not rotationally accelerated) contact_wrench[0:3] = (-model_com) / la.norm(model_com) * len_contact # rotate base accordingly (ore rather the whole robot) so gravity is parallel to contact force a_xz = np.array([0,9.81]) #xz of non-rotated contact force b_xz = contact_wrench[[0,2]] #rotated contact force vec projected to xz pitch = np.arccos( (a_xz.dot(b_xz))/(la.norm(a_xz)*la.norm(b_xz)) ) #angle in xz layer a_yz = np.array([0,9.81]) #yz of non-rotated contact force b_yz = contact_wrench[[1,2]] #rotated contact force vec projected to yz roll = np.arccos( (a_yz.dot(b_yz))/(la.norm(a_yz)*la.norm(b_yz)) ) #angle in yz layer yaw = 0 trajectory_data['base_rpy'][:] += np.array([roll, pitch, yaw]) """ trajectory_data['contacts'] = np.array( {contactFrame: np.tile(contact_wrench, (num_samples, 1))}) else: #TODO: add proper simulated contacts (from e.g. gazebo) for floating-base trajectory_data['contacts'] = np.array({}) if measurements: trajectory_data['positions'] = measurements['Q'] trajectory_data['velocities'] = measurements['V'] trajectory_data['accelerations'] = measurements['Vdot'] trajectory_data['measured_frequency'] = measurements[ 'measured_frequency'] old_skip = config['skipSamples'] config['skipSamples'] = 0 old_offset = config['startOffset'] config['startOffset'] = 0 data.init_from_data(trajectory_data) model.computeRegressors(data) trajectory_data['torques'][:, :] = data.samples['torques'][:, :] ''' if config['floatingBase']: # add force of contact to keep robot fixed in space (always accelerate exactly against gravity) # floating base orientation has to be rotated so that accelerations resulting from hanging # are zero, i.e. the vector COM - contact point is parallel to gravity. if contacts: # get jacobian of contact frame at current posture dim = model.num_dofs+fb jacobian = iDynTree.MatrixDynSize(6, dim) model.dynComp.getFrameJacobian(contactFrame, jacobian) jacobian = jacobian.toNumPy() # get base link vel and acc and torques that result from contact force / acceleration contacts_torq = np.zeros(dim) contacts_torq = jacobian.T.dot(contact_wrench) trajectory_data['base_acceleration'] += contacts_torq[0:6] # / 139.122 data.samples['base_acceleration'][:,:] = trajectory_data['base_acceleration'][:,:] # simulate again with proper base acceleration model.computeRegressors(data, only_simulate=True) trajectory_data['torques'][:,:] = data.samples['torques'][:,:] ''' config['skipSamples'] = old_skip config['startOffset'] = old_offset config['simulateTorques'] = old_sim return trajectory_data, data