def compute_primal_vrep(self): """Compute vertices of the primal tube.""" delta = self.target_com - self.start_com n = normalize(delta) t = array([0., 0., 1.]) t -= dot(t, n) * n t = normalize(t) b = cross(n, t) cross_section = [dx * t + dy * b for (dx, dy) in [ (+self.radius, +self.radius), (+self.radius, -self.radius), (-self.radius, +self.radius), (-self.radius, -self.radius)]] tube_start = self.start_com - self.margin * n tube_end = self.target_com + self.margin * n vertices = ( [tube_start + s for s in cross_section] + [tube_end + s for s in cross_section]) self.full_vrep = vertices if self.start_stance.label.startswith('SS'): if all(abs(self.start_stance.com.p - self.target_com) < 1e-3): self.primal_vrep = [vertices] else: # beginning of SS phase self.primal_vrep = [ [self.start_com], # single-support vertices] # ensuing double-support else: # start_stance is DS self.primal_vrep = [ vertices, # double-support [self.target_com]] # final single-support
def __init__(self, start_com, end_com, stance, radius, margin): n = normalize(end_com - start_com) t = array([0., 0., 1.]) t -= dot(t, n) * n t = normalize(t) b = cross(n, t) cross_section = [ dx * t + dy * b for (dx, dy) in [(+radius, +radius), (+radius, -radius), (-radius, +radius), (-radius, -radius)] ] tube_start = start_com - margin * n tube_end = end_com + margin * n primal_vrep = [tube_start + s for s in cross_section] + \ [tube_end + s for s in cross_section] primal_hrep = compute_polytope_hrep(primal_vrep) dual_vrep = stance.compute_pendular_accel_cone( com_vertices=primal_vrep) dual_hrep = compute_polytope_hrep(dual_vrep) self.dual_hrep = dual_hrep self.dual_vrep = dual_vrep self.primal_hrep = primal_hrep self.primal_vrep = primal_vrep
def compute_primal_vrep(self): """Compute vertices for both primal tubes.""" delta = self.target_com - self.start_com n = normalize(delta) t = array([0., 0., 1.]) t -= dot(t, n) * n t = normalize(t) b = cross(n, t) cross_section = [dx * t + dy * b for (dx, dy) in [ (+self.radius, +self.radius), (+self.radius, -self.radius), (-self.radius, +self.radius), (-self.radius, -self.radius)]] tube_start = self.start_com - self.margin * n tube_end = self.target_com + self.margin * n if self.start_stance.is_single_support: sep = self.start_stance.sep else: # self.start_stance.is_double_support sep = self.next_stance.sep vertices0, vertices1 = [], [] for s in cross_section: start_vertex = tube_start + s end_vertex = tube_end + s # NB: the order in the intersection matters, see function doc mid_vertex = intersect_line_cylinder(end_vertex, start_vertex, sep) if mid_vertex is None: # assuming that start_vertex is in the SEP, no intersection # means that both COM are inside the polygon vertices = ( [tube_start + s for s in cross_section] + [tube_end + s for s in cross_section]) if self.start_stance.is_single_support: # we are at the end of an SS phase self.primal_vrep = [vertices] else: # self.start_stance.is_double_support # we are in DS but polytope is included in the next SS-SEP self.primal_vrep = [vertices, vertices] return if self.start_stance.is_single_support: mid_vertex = start_vertex + 0.95 * (mid_vertex - start_vertex) vertices0.append(start_vertex) vertices0.append(mid_vertex) vertices1.append(start_vertex) vertices1.append(end_vertex) else: # self.start_stance.is_double_support mid_vertex = end_vertex + 0.95 * (mid_vertex - end_vertex) vertices0.append(start_vertex) vertices0.append(end_vertex) vertices1.append(mid_vertex) vertices1.append(end_vertex) self.primal_vrep = [vertices0, vertices1]
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 reset_state(scenario): contact_pos, contact_rpy, z_crit_des, zd = scenario contact.set_pos(contact_pos) contact.set_rpy(contact_rpy) pendulum.com.set_pos([0, 0, pendulum.z_target]) robot.ik.solve(max_it=100, impr_stop=1e-4) e_z = array([0., 0., 1.]) e_x = pendulum.target - pendulum.com.p e_x = normalize(e_x - dot(e_x, e_z) * e_z) z_diff = (pendulum.com.z - contact.z) - z_crit_des x = -dot(e_x, (contact.p - pendulum.com.p)) xd = -x / (2 * z_diff) * (-zd + sqrt(zd**2 + 2 * 9.81 * z_diff)) vel = xd * e_x + zd * e_z if '--gui' in sys.argv: global vel_handle vel_handle = draw_line(pendulum.com.p, pendulum.com.p + 0.5 * vel) pendulum.com.set_vel(vel) sim.step()
def update_state(self): """ Compute the stationary frame and current pendulum state in that frame. """ com = self.pendulum.com contact = self.pendulum.contact delta = com.p - contact.p e_z = array([0., 0., 1.]) e_x = -normalize(delta - dot(delta, e_z) * e_z) e_y = cross(e_z, e_x) R = vstack([e_x, e_y, e_z]) # from world to local frame p, pd = dot(R, delta), dot(R, com.pd) # in local frame T = transform_from_R_p(R.T, contact.p) # local to world frame self.T = T self.e_x = e_x self.e_y = e_y self.e_z = e_z self.state.set_pos(p) self.state.set_vel(pd)
friction=0.7) mass = 38. # [kg] z_target = 0.8 # [m] init_pos = array([0., 0., z_target]) vel = (0.7, 0.1, 0.2) draw_parabola = True if "--comanoid" in sys.argv: try: import comanoid vel = comanoid.setup(sim, robot, contact) draw_parabola = False except ImportError: warn("comanoid module not available, switching to default") delta = init_pos - contact.p e_z = array([0., 0., 1.]) e_x = -normalize(delta - dot(delta, e_z) * e_z) e_y = cross(e_z, e_x) init_vel = vel[0] * e_x + vel[1] * e_y + vel[2] * e_z pendulum = InvertedPendulum(mass, init_pos, init_vel, contact, z_target) pendulum.draw_parabola = draw_parabola stance = Stance(com=pendulum.com, right_foot=contact) stance.bind(robot) robot.ik.solve() g = -sim.gravity[2] lambda_min = 0.1 * g lambda_max = 2.0 * g stabilizer = Stabilizer3D( pendulum, lambda_min, lambda_max, nb_steps=10, cop_gain=2.) drawer = SolutionDrawer(stabilizer)