def generate_random_trajectory(robot, npts, maxd, randskip=0): # Use to skip configuration. Have no idea how to set random seed # in pinocchio. for i in range(randskip): pinocchio.randomConfiguration(robot.model) ts = [ 0.0, ] qs = [ pinocchio.randomConfiguration(robot.model), ] while len(qs) < npts: qlast = qs[-1] q = pinocchio.randomConfiguration(robot.model) d = pinocchio.distance(robot.model, qlast, q) if d > maxd: q = pinocchio.interpolate(robot.model, qlast, q, maxd / d) qs.append(q) ts.append(ts[-1] + pinocchio.distance(robot.model, qlast, q)) # Compute velocities and accelerations vs = [ np.zeros(robot.model.nv), ] accs = [ np.zeros(robot.model.nv), ] eps = 0.01 for q0, q1, q2 in zip(qs, qs[1:], qs[2:]): qprev = pinocchio.interpolate( robot.model, q0, q1, 1 - eps / pinocchio.distance(robot.model, q0, q1) ) qnext = pinocchio.interpolate( robot.model, q1, q2, eps / pinocchio.distance(robot.model, q1, q2) ) # (qnext - qprev) / eps vs.append(pinocchio.difference(robot.model, qprev, qnext) / eps) # ((qnext - q) - (q - qprev)) / eps^2 accs.append( ( pinocchio.difference(robot.model, q, qnext) - pinocchio.difference(robot.model, qprev, q) ) / (eps ** 2) ) vs.append(np.zeros(robot.model.nv)) accs.append(np.zeros(robot.model.nv)) from toppra.cpp import PiecewisePolyPath return PiecewisePolyPath.constructHermite(qs, vs, ts)
def diff(self, x0, x1): q0 = x0[:self.nq] q1 = x1[:self.nq] v0 = x0[-self.nv:] v1 = x1[-self.nv:] dq = pinocchio.difference(self.model, q0, q1) return np.concatenate([dq, v1 - v0])
def state_diff(robot, x1, x2): """ returns x2 - x1 """ xdiff = np.zeros([2 * robot.model.nv, 1]) xdiff[:robot.model.nv] = pin.difference(robot.model, x1[:robot.model.nq], x2[:robot.model.nq]) xdiff[robot.model.nv:] = x2[robot.model.nq:] - x1[robot.model.nq:] return xdiff
def compute_integration_errors(data, robot, dt): print('\n') res = Empty() res.ndt, res.dt, res.comp_time, res.realtime_factor = {}, {}, {}, {} res.err_2norm_avg, res.err_infnorm_avg, res.err_infnorm_max = {}, {}, {} res.err_traj_2norm, res.err_traj_infnorm = {}, {} res.mat_mult, res.mat_norm = {}, {} for name in sorted(data.keys()): if('ground-truth' in name): continue d = data[name] data_ground_truth = data['ground-truth-'+d.simulator] # if(d.simulator=='exponential'): data_ground_truth = data['ground-truth-exp'] # elif(d.simulator=='euler'): data_ground_truth = data['ground-truth-euler'] # elif(d.simulator=='implicit-euler'): data_ground_truth = data['ground-truth-implicit-euler'] # elif(d.simulator=='rk4'): data_ground_truth = data['ground-truth-rk4'] # else: raise BaseException('Ground truth method not recognized ') err_vec = np.empty((2*robot.nv, d.q.shape[1])) err_per_time_2 = np.empty(d.q.shape[1]) err_per_time_inf = np.empty(d.q.shape[1]) err_2, err_inf = 0.0, 0.0 for i in range(d.q.shape[1]): err_vec[:robot.nv,i] = pin.difference(robot.model, d.q[:,i], data_ground_truth.q[:,i]) err_vec[robot.nv:,i] = d.v[:,i] - data_ground_truth.v[:,i] err_per_time_2[i] = norm(err_vec[:,i]) err_per_time_inf[i] = norm(err_vec[:,i], np.inf) err_2 += err_per_time_2[i] err_inf += err_per_time_inf[i] err_2 /= d.q.shape[1] err_inf /= d.q.shape[1] err_peak = np.max(err_per_time_inf) print(name, 'Log error inf-norm: %.2f'%np.log10(err_inf)) if(d.method_name not in res.err_2norm_avg): for k in res.__dict__.keys(): res.__dict__[k][d.method_name] = [] res.err_2norm_avg[d.method_name] += [err_2] res.err_infnorm_avg[d.method_name] += [err_inf] res.err_infnorm_max[d.method_name] += [err_peak] res.err_traj_2norm[name] = err_per_time_2 res.err_traj_infnorm[name] = err_per_time_inf res.ndt[d.method_name] += [d.ndt] res.dt[d.method_name] += [dt/d.ndt] try: comp_time = d.computation_times['substep'].avg * d.ndt except: comp_time = np.nan res.comp_time[d.method_name] += [comp_time] res.realtime_factor[d.method_name] += [dt/comp_time] if(d.simulator=='exponential'): try: res.mat_mult[d.method_name] += [np.mean(d.mat_mult)] res.mat_norm[d.method_name] += [np.mean(d.mat_norm)] except: pass return res
def differentiatePostures(self, ini_state, end_state, dt): q_ini = np.transpose( np.matrix(ini_state.robot_posture.generalized_joint_positions())) q_end = np.transpose( np.matrix(end_state.robot_posture.generalized_joint_positions())) q_dot = pin.difference(self.robot.model, q_ini, q_end) / dt end_state.robot_velocity.generalized_joint_velocities = np.squeeze( np.asarray(q_dot)) return end_state
def dCostQ_dx(self, x): ''' ddiff_dx2(x1,x2) = dint_dv(x1,x2-x1)^-1 ddiff_dq( refQ,q) = dint_dv(refQ,q-refQ) ''' q = self.vq2q(a2m(x)) dq = pinocchio.difference(self.rmodel, self.refQ, q) dDiff = inv(pinocchio.dIntegrate(self.rmodel, self.refQ, dq)[1]) grad = dDiff[6:, :].T * dq[6:] return m2a(grad)
def diff(self, x0, x1): """ Difference between the robot's states x2 and x1. :param x1: current state :param x2: next state :return x2 [-] x1 value """ nq, nv, nx = self.model.nq, self.model.nv, self.nx assert (x0.shape == (nx, ) and x1.shape == (nx, )) q0 = x0[:nq] q1 = x1[:nq] v0 = x0[-nv:] v1 = x1[-nv:] dq = pinocchio.difference(self.model, a2m(q0), a2m(q1)) return np.concatenate([dq.flat, v1 - v0])
def test_basic(self): model = self.model q_ones = np.ones((model.nq)) self.assertFalse(pin.isNormalized(model, q_ones)) self.assertFalse(pin.isNormalized(model, q_ones, 1e-8)) self.assertTrue(pin.isNormalized(model, q_ones, 1e2)) q_rand = np.random.rand((model.nq)) q_rand = pin.normalize(model, q_rand) self.assertTrue(pin.isNormalized(model, q_rand)) self.assertTrue(pin.isNormalized(model, q_rand, 1e-8)) self.assertTrue(abs(np.linalg.norm(q_rand[3:7]) - 1.) <= 1e-8) q_next = pin.integrate(model, self.q, np.zeros((model.nv))) self.assertApprox(q_next, self.q) v_diff = pin.difference(model, self.q, q_next) self.assertApprox(v_diff, np.zeros((model.nv))) q_next = pin.integrate(model, self.q, self.v) q_int = pin.interpolate(model, self.q, q_next, 0.5) self.assertApprox(q_int, q_int) value = pin.squaredDistance(model, self.q, self.q) self.assertTrue((value <= 1e-8).all()) dist = pin.distance(model, self.q, self.q) self.assertTrue(dist <= 1e-8) q_neutral = pin.neutral(model) self.assertApprox(q_neutral, q_neutral) q_rand1 = pin.randomConfiguration(model) q_rand2 = pin.randomConfiguration(model, -np.ones((model.nq)), np.ones((model.nq))) self.assertTrue(pin.isSameConfiguration(model, self.q, self.q, 1e-8)) self.assertFalse(pin.isSameConfiguration(model, q_rand1, q_rand2, 1e-8))
def compute_integration_errors_vs_k_b(data, data_gt, robot, dt): print('\n') res = Empty() res.k, res.damping_ratio, res.comp_time, res.realtime_factor = {}, {}, {}, {} res.err_infnorm_avg, res.err_infnorm_max = {}, {} res.err_traj_infnorm = {} for name in sorted(data.keys()): if ('ground-truth' in name): continue d = data[name] data_ground_truth = data_gt[name] err_vec = np.empty((2 * robot.nv, d.q.shape[1])) err_per_time_inf = np.empty(d.q.shape[1]) err_inf = 0.0 for i in range(d.q.shape[1]): err_vec[:robot.nv, i] = pin.difference(robot.model, d.q[:, i], data_ground_truth.q[:, i]) err_vec[robot.nv:, i] = d.v[:, i] - data_ground_truth.v[:, i] err_per_time_inf[i] = norm(err_vec[:, i], np.inf) err_inf += err_per_time_inf[i] err_inf /= d.q.shape[1] err_peak = np.max(err_per_time_inf) if (d.method_name not in res.err_infnorm_avg): for k in res.__dict__.keys(): res.__dict__[k][d.method_name] = [] res.err_infnorm_avg[d.method_name] += [err_inf] res.err_infnorm_max[d.method_name] += [err_peak] res.err_traj_infnorm[name] = err_per_time_inf res.k[d.method_name] += [d.K] res.damping_ratio[d.method_name] += [d.damping_ratio] comp_time = d.computation_times['substep'].avg * d.ndt res.comp_time[d.method_name] += [comp_time] res.realtime_factor[d.method_name] += [dt / comp_time] try: print(name, 'Log error inf-norm: %.2f' % np.log10(err_inf), 'RT factor', int(dt / comp_time)) except: pass return res
def execute_motion(self, plotting=False, tune_online=False): sim = self.sim P, D = 10. * np.ones(8), 0.3 * np.ones(8) for i in range(8): if "HFE" in self.robot.model.names[int(sim.pinocchio_joint_ids[i])]: D[i] = 0.5 if "FR" in self.robot.model.names[int(sim.pinocchio_joint_ids[i])] or "FL" in self.robot.model.names[int(sim.pinocchio_joint_ids[i])]: D[i] = 0.5 num_uncontrolled_joints = 6 # Apply gains to reach steady state loop = 0 try: while loop < 2000: q, dq = sim.get_state() ptau = np.diag(P) * se3.difference(self.robot.model, q, self.init_config)[6:] ptau += np.diag(D) * -dq[6:] self.limit_torques(ptau) sim.send_joint_command(ptau) sim.step() loop += 1 except KeyboardInterrupt: print("Keyboard interrupt") desired_pos = desired_state("POSITION", self.time_vector, optimized_sequence=self.optimized_kin_plan) desired_vel = desired_state("VELOCITY", self.time_vector, optimized_sequence=self.optimized_kin_plan) desired_com_func = desired_state("COM", self.time_vector, optimized_sequence=self.optimized_kin_plan) desired_lmom_func = desired_state("LMOM", self.time_vector, optimized_sequence=self.optimized_kin_plan) desired_amom_func = desired_state("AMOM", self.time_vector, optimized_sequence=self.optimized_kin_plan) if not self.dynamics_feedback is None: dyn_feedback = desired_state("DYN_FEEDBACK", self.time_vector, dynamics_feedback=self.dynamics_feedback) time_horizon = 4.0 max_num_iterations = int(time_horizon * 1000) desired_pos_arr = np.zeros((max_num_iterations, len(self.controlled_joints))) desired_vel_arr = np.zeros((max_num_iterations, len(self.controlled_joints))) actual_pos_arr = np.zeros((max_num_iterations, len(self.controlled_joints))) actual_vel_arr = np.zeros((max_num_iterations, len(self.controlled_joints))) base_states = np.zeros((max_num_iterations, 7)) tau_arr = np.zeros((max_num_iterations, len(self.controlled_joints))) forces_arr = np.zeros((max_num_iterations, len(sim.pinocchio_endeff_ids), 6)) t_vec = np.zeros((max_num_iterations)) # t_0 = time() robot_weight = self.planner_setting.get(PlannerDoubleParam_RobotWeight) jacobians_eff = {} for eff in self.robot.effs: for joint in self.robot.joints_list: joint_identifier = eff + "_" + joint jacobians_eff[joint_identifier] = self.robot.get_jacobian(joint_identifier, "TRANSLATION") # Apply gains for trajectory tracking try: print("Executing motion...") executing = True # t_0 = None # t_1 = None force_offset = 0 while executing: loop = 0 time_id = 0 swing_times = {} for eff in self.robot.effs: swing_times[eff] = [] while loop < max_num_iterations: t = loop / 1e3 if t > self.time_vector[time_id]: time_id += 1 des_pos = desired_pos(t) des_vel = desired_vel(t) q, dq = sim.get_state() frame_ids, forces = sim.get_force() q_des = q.copy() q_des[7:] = des_pos.reshape((-1, 1)) dq_des = dq.copy() dq_des[6:] = des_vel.reshape((-1, 1)) ptau = np.diag(P) * se3.difference(self.robot.model, q, q_des)[6:] ptau += np.diag(D) * (dq_des - dq)[6:] planned_force = np.zeros((3 * len(self.robot.effs))) jacobians_effs = np.zeros((3 * len(self.robot.effs), self.robot.robot.nv)) for eff_id, eff in enumerate(self.robot.effs): eff = eff + "_ANKLE" force = self.optimized_dyn_plan.dynamics_states[time_id].effForce(eff_id) * robot_weight # force = self.optimized_dyn_plan.dynamics_states[min(time_id - force_offset, len(self.time_vector) - 1)].effForce(eff_id) * robot_weight # force = self.optimized_dyn_plan.dynamics_states[max(time_id - force_offset, 0)].effForce(eff_id) * robot_weight planned_force[eff_id * 3 : eff_id * 3 + 3] = force jacobians_effs[eff_id * 3 : eff_id * 3 + 3, :] = jacobians_eff[eff]() if self.dynamics_feedback is None: use_dyn_feedback = False else: use_dyn_feedback = True if use_dyn_feedback: if loop == 0: delta_t = 0 else: delta_t = t - t_vec[-1] joint_configuration = q[7:].reshape((-1)) base_state = np.zeros((7)) base_state[:3], base_state[3:] = p.getBasePositionAndOrientation(self.robotId) com, lmom, amom = self.calculate_momentum(delta_t, joint_configuration, base_state) desired_momentum = np.hstack((desired_com_func(t), desired_lmom_func(t), desired_amom_func(t))) current_momentum = np.hstack((com, lmom, amom)) delta_momentum = current_momentum - desired_momentum delta_force = np.dot(dyn_feedback(t), delta_momentum) else: delta_force = np.zeros_like(planned_force) ptau += np.reshape(np.transpose(np.dot(np.transpose(jacobians_effs), planned_force + delta_force))[6:], (ptau.shape[0], 1)) self.limit_torques(ptau) sim.send_joint_command(ptau) desired_pos_arr[loop, :] = des_pos desired_vel_arr[loop, :] = des_vel actual_pos_arr[loop, :] = q[7:].reshape((-1)) actual_vel_arr[loop, :] = dq[6:].reshape((-1)) base_state_and_orientation = p.getBasePositionAndOrientation(self.robotId) base_states[loop, :3] = base_state_and_orientation[0] base_states[loop, 3:] = base_state_and_orientation[1] t_vec[loop] = t tau_arr[loop, :] = np.squeeze(np.array(ptau), 1) for cnt_idx in range(len(forces)): endeff_id = np.where(np.array(sim.pinocchio_endeff_ids) == frame_ids[cnt_idx])[0][0] forces_arr[loop, endeff_id, :] = forces[cnt_idx] robot_endeff = self.robot.effs[endeff_id] # Determine swing times if np.sum(np.abs(forces[cnt_idx])) < 1e-3: if len(swing_times[robot_endeff]) == 0: swing_times[robot_endeff] = [[t]] elif len(swing_times[robot_endeff][-1]) == 2: swing_times[robot_endeff].append([t]) else: if len(swing_times[robot_endeff]) > 0: if len(swing_times[robot_endeff][-1]) == 1: swing_times[robot_endeff][-1].append(t) sim.step() # sleep(0.001) loop += 1 # force_offset += 1 actual_com, actual_lmom, actual_amom = self.calculate_actual_trajectories(loop, t_vec, actual_pos_arr, base_states) desired_com = np.zeros((len(self.time_vector), 3)) desired_lmom = np.zeros((len(self.time_vector), 3)) desired_amom = np.zeros((len(self.time_vector), 3)) for t in range(len(self.time_vector)): desired_com[t, :] = self.optimized_kin_plan.kinematics_states[t].com desired_lmom[t, :] = self.optimized_kin_plan.kinematics_states[t].lmom desired_amom[t, :] = self.optimized_kin_plan.kinematics_states[t].amom # Apply delta to desired_com, because of different initial positions desired_com += actual_com[0, :] - desired_com[0, :] actual_trajectories = {"joint_configs": actual_pos_arr, "joint_velocities": actual_vel_arr, "COM": actual_com, "LMOM": actual_lmom, "AMOM": actual_amom} desired_trajectories = {"joint_configs": desired_pos_arr, "joint_velocities": desired_vel_arr, "COM": desired_com, "LMOM": desired_lmom, "AMOM": desired_amom} if plotting: self.plot_execution(t_vec, loop, desired_trajectories, actual_trajectories, swing_times) self.plot_torques(t_vec, loop, tau_arr, swing_times) self.plot_forces(t_vec, loop, forces_arr, swing_times) if tune_online: P, D = self.tunePD(P, D) else: executing = False # pass print("...Finished execution.") except KeyboardInterrupt: print("Keyboard interrupt")
def costQ(self, x): q = self.vq2q(a2m(x)) self.residuals = m2a( pinocchio.difference(self.rmodel, self.refQ, q)[6:]) return sum(self.residuals**2)
def q2vq(self, q): return pinocchio.difference(self.rmodel, self.q0, q)
def residual(self, q): return pin.difference(self.rmodel, q, self.qref)
def calc(self, q): self.res = self.weights * pin.difference(self.rmodel, self.desired_posture, q) return self.res
def get_difference(self, q_1, q_2): return pinocchio.difference(self.model, q_1, q_2)
def costQ(self, x): q, tauq, fr, fl = self.x2qf(x) self.residuals[:] = pinocchio.difference(self.rmodel, self.refQ, q)[6:].flat return sum(self.residuals**2)