def update_target_pose(self, dt): assert self.retimed_traj is not None from_start = self.foot_link.p - self.start_pose[4:] to_goal = self.end_pose[4:] - self.foot_link.p dist_to_goal = norm(to_goal) dist_from_start = norm(from_start) x = dist_to_goal / (dist_from_start + dist_to_goal) quat = quat_slerp(self.foot.quat, self.end_pose[0:4], 1. - x) self.foot_vel = self.retimed_traj.Evald(dt) self.foot.set_pos(self.retimed_traj.Eval(dt)) self.foot.set_quat(quat)
def on_tick(self, sim): if self.handle is None: self.update_cone() for contact in self.stance.contacts: if norm(contact.pose - self.contact_poses[contact.name]) > 1e-10: self.update_contact_poses() self.update_cone() break if norm(self.stance.com.p - self.last_com) > 1e-10: self.update_contact_poses() self.update_cone() self.last_com = self.stance.com.p
def xprod_ratio(self): """Used to test the validity of our assumption on neglecting Dz x Dp.""" t = 0. for k in xrange(self.nb_steps): contact = self.contacts[k] Delta_p = self.Delta_X[k, 0:3] Delta_z = self.Delta_Z[k] for (i, v) in enumerate(contact.vertices): e1 = cross(v - self.P_ref[k], Delta_z) e2 = cross(self.Z_ref[k] - v, Delta_p) min_norm = norm(e1 + e2) t += norm(cross(Delta_p, Delta_z)) / min_norm return t / self.nb_steps
def interpolate_foot_path(self): p0 = self.foot.p p1 = self.end_pose[4:] R = rotation_matrix_from_quat(self.end_pose[:4]) t, n = R[0:3, 0], R[0:3, 2] v1 = 0.5 * t - 0.5 * n if norm(self.foot_vel) > 1e-4: v0 = self.foot_vel self.path = interpolate_uab_hermite_topp(p0, v0, p1, v1) self.sd_beg = norm(v0) / norm(self.path.Evald(0.)) else: # choose initial direction v0 = 0.3 * self.foot.t + 0.7 * self.foot.n self.path = interpolate_uab_hermite_topp(p0, v0, p1, v1) self.sd_beg = 0.
def solve_nlp(self): t_solve_start = time() X = self.nlp.solve() self.solve_time = time() - t_solve_start # print "Symbols:", self.nlp.var_symbols N = self.nb_steps Y = X[:-6].reshape((N, 3 + 3 + 3 + 1 + 16)) P = Y[:, 0:3] V = Y[:, 3:6] # U = Y[:, 6:9] # not used dT = Y[:, 9] L = Y[:, 10:26] p_last = X[-6:-3] v_last = X[-3:] Z, Ld = self.compute_Z_and_Ld(P, L) cp_nog_last = p_last + v_last / self.omega cp_nog_last_des = self.end_com + self.end_comd / self.omega cp_error = norm(cp_nog_last - cp_nog_last_des) T_swing = sum(dT[:self.nb_steps / 2]) if self.swing_duration is not None and \ T_swing > 1.25 * self.swing_duration: self.update_swing_dT_min(self.swing_dT_min / 2) self.cp_error = cp_error if self.cp_error > 0.1: # and not self.preview.is_empty: print("Warning: preview not updated as CP error was", cp_error) self.nlp.warm_start(X) self.preview.update(P, V, Z, dT, self.omega2) self.p_last = p_last self.v_last = v_last self.print_results()
def compute_geom_reachable_polygon(robot, stance, xlim, ylim, draw=True): init_com = stance.com.p feasible_points = [] handles = [] points = generate_2d_grid(xlim, ylim, xres=4, yres=6) for point in points: stance.com.set_x(init_com[0] + point[0]) stance.com.set_y(init_com[1] + point[1]) robot.ik.solve(warm_start=True) # self.draw_polytope_slice() # draw_polygon() if norm(robot.com - stance.com.p) < 0.02: feasible_points.append(robot.com) if draw: handles.append(draw_point(robot.com, pointsize=5e-3)) # handle.append(draw_point(stance.com.p)) feasible = [array([p[0], p[1]]) for p in feasible_points] hull = ConvexHull(feasible) vertices = [feasible[i] for i in hull.vertices] if draw: z_avg = pylab.mean([p[2] for p in feasible_points]) vertices_3d = [array([v[0], v[1], z_avg]) for v in vertices] handles.extend([draw_point(v) for v in vertices_3d]) stance.com.set_pos(init_com) return vertices
def solve_nlp(self): t_solve_start = time() X = self.nlp.solve() self.solve_time = time() - t_solve_start # print "Symbols:", self.nlp.var_symbols N = self.nb_steps Y = X[:-6].reshape((N, 3 + 3 + 3 + 3 + 1)) P = Y[:, 0:3] V = Y[:, 3:6] # U = Y[:, 6:9] # not used Z = Y[:, 9:12] dT = Y[:, 12] p_last = X[-6:-3] v_last = X[-3:] cp_nog_last = p_last + v_last / self.omega cp_nog_last_des = self.end_com + self.end_comd / self.omega cp_error = norm(cp_nog_last - cp_nog_last_des) T_swing = sum(dT[:self.nb_steps / 2]) if self.swing_duration is not None and \ T_swing > 1.25 * self.swing_duration: self.update_swing_dT_min(self.swing_dT_min / 2) self.cp_error = cp_error if self.cp_error > 0.1: # and not self.preview.is_empty: print "Warning: preview not updated as CP error was", cp_error return self.nlp.warm_start(X) # save as initial guess for next iteration self.preview.update(P, V, Z, dT, self.omega2) self.p_last = p_last self.v_last = v_last self.print_results()
def interpolate_path(self): target_p = self.fsm.next_stance.com target_pd = self.fsm.next_stance.comd p0 = self.body.p p1 = target_p if norm(p1 - p0) < 1e-3: return None, None v1 = target_pd if norm(self.body.pd) > 1e-4: v0 = self.body.pd path = interpolate_houba_topp(p0, p1, v0, v1, hack=True) sd_beg = norm(v0) / norm(path.Evald(0.)) else: # choose initial direction delta = normalize(p1 - p0) ref = normalize(self.fsm.cur_stance.comd) v0 = 0.7 * delta + 0.3 * ref path = interpolate_houba_topp(p0, p1, v0, v1, hack=True) sd_beg = 0. return path, sd_beg
def on_tick(self, sim): if self.finished: return elif norm(self.end_pose[4:] - self.foot.p) < 5e-3: self.finished = True self.foot.set_pose(self.end_pose) self.foot_vel *= 0. return self.compute_trajectory() self.update_target_pose(sim.dt)
def interpolate_path(self): target_pose = self.body.end_pose target_p = target_pose[4:] R = rotation_matrix_from_quat(target_pose[:4]) t, n = R[0:3, 0], R[0:3, 2] target_pd = 0.5 * t - 0.5 * n p0 = self.body.p p1 = target_p if norm(p1 - p0) < 1e-3: return None, None v1 = target_pd if norm(self.body.pd) > 1e-4: v0 = self.body.pd path = interpolate_houba_topp(p0, p1, v0, v1) sd_beg = norm(v0) / norm(path.Evald(0.)) else: # choose initial direction v0 = 0.3 * self.body.t + 0.7 * self.body.n path = interpolate_houba_topp(p0, p1, v0, v1) sd_beg = 0. return path, sd_beg
def on_tick(self, sim): if self.handle is None: self.create_polyhedron(self.stance.contacts) return for contact in self.stance.contacts: if norm(contact.pose - self.contact_poses[contact.name]) > 1e-10: self.update_contacts() self.create_polyhedron(self.stance.contacts) return if self.nr_iter < self.max_iter: self.refine_polyhedron() self.nr_iter += 1
def on_tick(self, sim): """ Update the FSM state after a tick of the control loop. INPUT: - ``sim`` -- instance of current simulation """ if self.is_over: return if self.cur_stance.is_single_support: foot_target = self.next_stance.target_foot.effector_pose[4:] if norm(self.free_foot.p - foot_target) < 0.01: self.step() else: # self.cur_stance.is_double_support # dist_to_edge = self.next_stance.dist_to_sep_edge(self.com.p) close_to_target = norm(self.cur_stance.com - self.com.p) < 0.05 # small_velocity = norm(self.com.pd) < 0.1 if close_to_target: self.com.set_velocity(0. * self.com.pd) # kron self.step()
def on_tick(self, sim): """ Apply regular impulses to the inverted pendulum. Parameters ---------- sim : pymanoid.Simulation Simulation instance. """ self.nb_ticks += 1 if self.handle is not None and self.nb_ticks % 15 == 0: self.handle = None if self.nb_ticks % 50 == 0: com = self.pendulum.com.p comd = self.pendulum.com.pd dv = 2. * numpy.random.random(3) - 1. dv[2] *= 0.5 # push is weaker in vertical direction dv *= self.gain / norm(dv) self.pendulum.com.set_vel(comd + dv) self.handle = draw_arrow(com - dv, com, color='b', linewidth=0.01)
sim.schedule(pendulum) sim.schedule(robot.ik) sim.step() nb_launches, nb_samples = 0, 0 while nb_launches < TOTAL_LAUNCHES or nb_samples < TOTAL_SAMPLES: scenario = sample_scenario() for stabilizer in [stabilizer_2d, stabilizer_3d]: stabilizer_2d.pause() stabilizer_3d.pause() stabilizer.resume() reset_state(scenario) if not stabilizer.solver.optimal_found: warn("Unfeasible sample") continue while norm(pendulum.com.p - pendulum.target) > 2e-3: if not stabilizer.solver.optimal_found or stabilizer.T is None: error("Unfeasible state encountered during execution") break sim.step() print "\n------------------------------------------\n" print "Launch %d for %s" % (nb_launches + 1, type(stabilizer).__name__) stabilizer.solver.print_debug_info() nb_2d_samples = len(stabilizer_2d.solver.solve_times) nb_3d_samples = len(stabilizer_3d.solver.solve_times) nb_samples = min(nb_2d_samples, nb_3d_samples) nb_launches += 1 print "Results" print "-------"
def on_tick(self, sim): if norm(self.robot.com - self.last_com) > 1e-2: self.last_com = self.robot.com self.update_polygon() if self.handle is None: self.update_handle()
def on_tick(self, sim): if norm(self.pendulum.com.p - self.pendulum.target) < 2e-3: info("Stopping simulation as pendulum converged") sim.stop()