def main(): # save either optimized or random trajectory parameters to filename if args.filename: traj_file = args.filename else: traj_file = config['urdf'] + '.trajectory.npz' if config['optimizeTrajectory']: # find trajectory params by optimization old_sim = config['simulateTorques'] config['simulateTorques'] = True model = Model(config, config['urdf']) if config['useStaticTrajectories']: old_gravity = config['identifyGravityParamsOnly'] idf = Identification(config, config['urdf'], config['urdf_real'], measurements_files=None, regressor_file=None, validation_file=None) trajectoryOptimizer = PostureOptimizer(config, idf, model, simulation_func=simulateTrajectory, world=args.world) config['identifyGravityParamsOnly'] = old_gravity else: idf = Identification(config, config['urdf'], urdf_file_real=None, measurements_files=None, regressor_file=None, validation_file=None) trajectoryOptimizer = TrajectoryOptimizer(config, idf, model, simulation_func=simulateTrajectory, world=args.world) trajectory = trajectoryOptimizer.optimizeTrajectory() config['simulateTorques'] = old_sim else: # use some random params print("no optimized trajectory found, generating random one") trajectory = PulsedTrajectory(config['num_dofs'], use_deg=config['useDeg']).initWithRandomParams() print("a {}".format([t_a.tolist() for t_a in trajectory.a])) print("b {}".format([t_b.tolist() for t_b in trajectory.b])) print("q {}".format(trajectory.q.tolist())) print("nf {}".format(trajectory.nf.tolist())) print("wf {}".format(trajectory.w_f_global)) print("Saving found trajectory to {}".format(traj_file)) if config['useStaticTrajectories']: # always saved with rad angles np.savez(traj_file, static=True, angles=trajectory.angles) else: # TODO: remove degrees option np.savez(traj_file, use_deg=trajectory.use_deg, static=False, a=trajectory.a, b=trajectory.b, q=trajectory.q, nf=trajectory.nf, wf=trajectory.w_f_global)
config = yaml.load(stream) except yaml.YAMLError as exc: print(exc) import iDynTree iDynTree.init_helpers() iDynTree.init_numpy_helpers() dynComp = iDynTree.DynamicsComputations() dynComp.loadRobotModelFromFile(args.model) world_gravity = iDynTree.SpatialAcc.fromList([0, 0, -9.81, 0, 0, 0]) n_dof = dynComp.getNrOfDegreesOfFreedom() config['num_dofs'] = n_dof config['urdf'] = args.model g_model = Model(config, args.model, regressor_file=None, regressor_init=False) linkNames = g_model.linkNames # get bounding boxes for model from identification.helpers import URDFHelpers, ParamHelpers paramHelpers = ParamHelpers(None, config) urdfHelpers = URDFHelpers(paramHelpers, None, config) link_cuboid_hulls = {} # type: Dict[str, Tuple[List, List, List]] for i in range(len(linkNames)): link_name = linkNames[i] box, pos, rot = urdfHelpers.getBoundingBox(input_urdf=args.model, old_com=[0, 0, 0], link_name=link_name, scaling=False)
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'], velocities_raw=out['velocities'],
def __init__(self, opt, urdf_file, urdf_file_real, measurements_files, regressor_file, validation_file): # type: (Dict, str, str, str, str, str) -> None self.opt = opt # some additional options (experiments) # in order ot get regressor and base equations, use basis projection matrix. Otherwise use # permutation from QR directly (Gautier/Sousa method) self.opt['useBasisProjection'] = 0 # in case projection is used, orthogonalize the basis matrix (SDP estimation seem to work # more stable that way) self.opt['orthogonalizeBasis'] = 1 # add regularization term to SDP identification that minimized CAD distance for non-identifiable params self.opt['useRegressorRegularization'] = 1 self.opt['regularizationFactor'] = 1000.0 #proportion of distance term # if using fixed base dynamics, remove first link that is the fixed base which should completely # not be identifiable and not be part of equations (as it does not move) self.opt['deleteFixedBase'] = 1 # end additional config flags # load model description and initialize self.model = Model(self.opt, urdf_file, regressor_file) # load measurements self.data = Data(self.opt) if measurements_files: self.data.init_from_files(measurements_files) self.paramHelpers = helpers.ParamHelpers(self.model, self.opt) self.urdfHelpers = helpers.URDFHelpers(self.paramHelpers, self.model, self.opt) self.sdp = sdp.SDP(self) if self.opt['constrainUsingNL']: from identification.nlopt import NLOPT self.nlopt = NLOPT(self) self.tauEstimated = None # type: np._ArrayLike self.res_error = 100 # last residual error in percent self.urdf_file_real = urdf_file_real if self.urdf_file_real: dc = iDynTree.DynamicsRegressorGenerator() if not dc.loadRobotAndSensorsModelFromFile(urdf_file_real): sys.exit() tmp = iDynTree.VectorDynSize(self.model.num_model_params) #set regressor, otherwise getModelParameters segfaults dc.loadRegressorStructureFromString(self.model.regrXml) dc.getModelParameters(tmp) self.xStdReal = tmp.toNumPy() #add some zeros for friction self.xStdReal = np.concatenate((self.xStdReal, np.zeros(self.model.num_all_params-self.model.num_model_params))) if self.opt['identifyFriction']: self.paramHelpers.addFrictionFromURDF(self.model, self.urdf_file_real, self.xStdReal) self.validation_file = validation_file progress_inst = helpers.Progress(opt) self.progress = progress_inst.progress
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