def on_tick(self, sim): if preview_buffer.preview is None: return com_pre, comd_pre = com_target.p, com_target.pd com_free, comd_free = com_target.p, com_target.pd dT = preview_buffer.preview.timestep self.handles = [] self.handles.append( draw_point(com_target.p, color='m', pointsize=0.007)) for preview_index in xrange(len(preview_buffer.preview.U) / 3): com_pre0 = com_pre j = 3 * preview_index comdd = preview_buffer.preview.U[j:j + 3] com_pre = com_pre + comd_pre * dT + comdd * .5 * dT ** 2 comd_pre += comdd * dT color = \ 'b' if preview_index <= preview_buffer.preview.switch_step \ else 'y' self.handles.append( draw_point(com_pre, color=color, pointsize=0.005)) self.handles.append( draw_line(com_pre0, com_pre, color=color, linewidth=3)) if self.draw_free_traj: com_free0 = com_free com_free = com_free + comd_free * dT self.handles.append( draw_point(com_free, color='g', pointsize=0.005)) self.handles.append( draw_line(com_free0, com_free, color='g', linewidth=3))
def draw(self): com, comd = self.com_state.p, self.com_state.pd cp = com + comd / self.omega + gravity / self.omega2 zmp = self.zmp_state.p handles = [ draw_point(com, color='r', pointsize=0.025), draw_point(cp, color='b', pointsize=0.025), draw_point(zmp, color='r', pointsize=0.025), draw_line(com, cp, color='b', linewidth=1), draw_line(zmp, com, color='r', linewidth=4) ] return handles
def play(self, sim, slowdown=1., wrench_drawer=None, post_preview_duration=None, callback=None): handles = None com, comd = self.P[0], self.V[0] if wrench_drawer is not None: wrench_drawer.point_mass.set_pos(com) wrench_drawer.point_mass.set_vel(comd) wrench_drawer.point_mass.set_transparency(0.5) t, k, rem_time = 0., -1, -1., duration = 2 * self.duration if post_preview_duration is None else \ self.duration + post_preview_duration while t <= duration: if rem_time < 1e-10: if k < self.nb_steps - 1: k += 1 rem_time = self.get_dT(k) omega2 = self.get_omega2(k) cp = com + comd / sqrt(omega2) + sim.gravity / omega2 zmp = self.get_zmp(k) if k < self.nb_steps else cp comdd = omega2 * (com - zmp) + sim.gravity handles = [ draw_point(zmp, color='r'), draw_line(zmp, com, color='r', linewidth=4), draw_point(cp, color='b'), draw_line(cp, com, color='b', linewidth=2), draw_point(com, color='r', pointsize=0.05) ] if wrench_drawer is not None: try: wrench_drawer.recompute(self.get_contact(k), comdd, am=None) except ValueError: print "Warning: wrench validation failed at t = %.2f s" % t dt = min(rem_time, sim.dt) com, comd = integrate_fip(com, comd, zmp, dt, omega2) if wrench_drawer is not None: wrench_drawer.point_mass.set_pos(com) wrench_drawer.point_mass.set_vel(comd) sleep(slowdown * dt) if callback is not None: callback() rem_time -= dt t += dt if wrench_drawer is not None: wrench_drawer.handles = [] return handles
def draw(self, step=0.02): self.handles['leg'] = draw_line(self.com.p, self.contact.p + self.cop, linewidth=4, color='g') self.handles['target'] = draw_line(self.contact.p, self.target, 'b') self.handles['target_p'] = draw_point(self.target, 'b', pointsize=0.01) if not __debug__ or not self.draw_parabola: return p, t = self.com.p, 0. parabola = [] while p[2] > self.com.z - 1: p = self.com.p + self.com.pd * t + gravity * t**2 / 2 parabola.append(p) t += step if not parabola: return self.handles['parabola'] = draw_trajectory(parabola, pointsize=0)
def draw(self, color='b', pointsize=0.005): handles = [] if self.is_empty: return handles com, comd = self.P[0], self.V[0] handles.append(draw_point(com, color, pointsize)) for k in xrange(self.nb_steps): com_prev = com dT = self.get_dT(k) omega2 = self.get_omega2(k) zmp = self.get_zmp(k) com, comd = integrate_fip(com, comd, zmp, dT, omega2) handles.append(draw_point(com, color, pointsize)) handles.append(draw_line(com_prev, com, color, linewidth=3)) com_prev = com com = com + comd / sqrt(omega2) # stationary value at CP handles.append(draw_point(com, color, pointsize)) handles.append(draw_line(com_prev, com, color, linewidth=3)) return handles
def compute_surface_frame(self): contact = self.pendulum.contact dot_ez_n = dot(self.e_z, contact.n) phi = arctan(-dot(self.e_x, contact.n) / dot_ez_n) theta = arctan(-dot(self.e_y, contact.n) / dot_ez_n) t = cos(phi) * self.e_x + sin(phi) * self.e_z b = cos(theta) * self.e_y + sin(theta) * self.e_z if __debug__: self.__frames = [ draw_line(contact.p, contact.p + .2 * self.e_x, color='r'), draw_line(contact.p, contact.p + .2 * self.e_y, color='g'), draw_line(contact.p, contact.p + .2 * self.e_z, color='b'), # draw_line(contact.p, contact.p + .2 * t, color='m'), # draw_line(contact.p, contact.p + .2 * b, color='k'), draw_line(contact.p, contact.p + .2 * contact.n, color='c') ] self.R_surf = vstack([t, b, contact.n]).T self.t = t self.b = b self.phi = phi self.theta = theta
def on_tick(self, sim): """ Entry point called at each simulation tick. Parameters ---------- sim : Simulation Instance of the current simulation. """ if preview_buffer.is_empty: return com_pre, comd_pre = com_target.p, com_target.pd com_free, comd_free = com_target.p, com_target.pd self.handles = [] self.handles.append( draw_point(com_target.p, color='m', pointsize=0.007)) for preview_index in xrange(preview_buffer.nb_steps): com_pre0 = com_pre j = 3 * preview_index comdd = preview_buffer._U[j:j + 3] dT = preview_buffer._dT[preview_index] com_pre = com_pre + comd_pre * dT + comdd * .5 * dT**2 comd_pre += comdd * dT color = \ 'b' if preview_index <= preview_buffer.switch_step \ else 'y' self.handles.append( draw_point(com_pre, color=color, pointsize=0.005)) self.handles.append( draw_line(com_pre0, com_pre, color=color, linewidth=3)) if self.draw_free_traj: com_free0 = com_free com_free = com_free + comd_free * dT self.handles.append( draw_point(com_free, color='g', pointsize=0.005)) self.handles.append( draw_line(com_free0, com_free, color='g', linewidth=3))
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 draw(self): """ Draw the interpolated foot path. Returns ------- handle : openravepy.GraphHandle OpenRAVE graphical handle. Must be stored in some variable, otherwise the drawn object will vanish instantly. """ foot_path = self.retimed_traj.path if foot_path is None: return [] ds = self.discrtimestep handles = [draw_point(foot_path.Eval(0), color='m', pointsize=0.007)] for s in arange(ds, foot_path.duration + ds, ds): handles.append(draw_point( foot_path.Eval(s), color='b', pointsize=0.01)) handles.append(draw_line( foot_path.Eval(s - ds), foot_path.Eval(s), color='b', linewidth=3)) return handles
def draw_comdd(self): comdd = self.acc_scale * preview_buffer.cur_control + self.trans self.comdd_handle = [ draw_line(self.trans, comdd, color='r', linewidth=3), draw_points([self.trans, comdd], color='r', pointsize=0.005) ]
def draw_comdd(self): comdd = self.acc_scale * preview_buffer.cur_control + self.trans self.comdd_handle = [ draw_line(self.trans, comdd, color='r', linewidth=3), draw_points([self.trans, comdd], color='r', pointsize=0.005)]
def on_tick(self, sim): self.update_pendulum_velocity() self.line_handle = draw_line( self.pendulum.com.p, self.end_point.p, linewidth=3)