def draw_solution(self):
if self.solver is None or self.T is None:
return []
N, solver = self.solver.N, self.solver
if solver.lambda_ is None:
solver.compute_full_trajectory()
virt_pendulum = self.pendulum.copy()
virt_pendulum.hide()
max_time = solver.switch_times[-1] * 1.1
omega_i = solver.get_omega_i()
k = self.cop_gain
r_i = k * (self.state.p + self.state.pd / omega_i)
alpha_i = r_i[0] / cos(self.phi)
beta_i = r_i[1] / cos(self.theta)
points = []
for i in xrange(solver.N):
t_i = solver.switch_times[i]
t_next = solver.switch_times[i + 1] if i < N - 1 else max_time
virt_pendulum.set_lambda(solver.lambda_[N - i - 1])
dt = (t_next - t_i) / 10.
for t in arange(t_i, t_next, dt):
s = solver.s_from_t(t)
omega_t = solver.omega_from_t(t)
alpha = alpha_i * (s * omega_t / omega_i)**(k - 1)
beta = beta_i * (s * omega_t / omega_i)**(k - 1)
cop = dot(self.R_surf, [alpha, beta, 0.])
virt_pendulum.set_cop(cop)
virt_pendulum.integrate(dt)
points.append(virt_pendulum.com.p)
points.append(self.pendulum.target)
return draw_trajectory(points, color='m')
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_solution(self):
if self.solver is None or self.T is None:
return []
N, solver = self.solver.N, self.solver
if solver.lambda_ is None:
solver.compute_full_trajectory()
origin = pymanoid.Contact(shape=(1e-3, 1e-3), pos=[0, 0, 0])
p, pd = self.state.p, self.state.pd
virt_pendulum = InvertedPendulum(1., p, pd, origin, None)
virt_pendulum.hide()
points = []
max_time = solver.switch_times[-1] * 2
for i in xrange(solver.N):
t_i = solver.switch_times[i]
t_next = solver.switch_times[i + 1] if i < N - 1 else max_time
virt_pendulum.set_lambda(solver.lambda_[N - i - 1])
virt_pendulum.integrate(t_next - t_i)
points.append(apply_transform(self.T, virt_pendulum.com.p))
return draw_trajectory(points, color='c')