def main(): # first let's build a system: sys = DeltaRobotTrep(L_base, L_platform, R_base, R_platform) dt = 0.05 tf = 10.0 # setup initial conditions _ = sys.forward(*(-0.2 * np.ones(3))) _ = sys.inverse(0.0, 0.0, 1.2) q0 = sys.q # add external force: trep.forces.BodyWrench(sys, sys.platform_center, (0, 0, "external_z", 0, 0, 0)) def f_func(t): if t > 3.0: return 50 else: return 0 # amp = 20 # period = 1.0 # return amp*np.sin(t*2*pi/period) + 40 mvi = trep.MidpointVI(sys) mvi.initialize_from_configs(0.0, q0, dt, q0) q = [mvi.q2] t = [mvi.t2] while mvi.t1 < tf: mvi.step(mvi.t2 + dt, u1=[f_func(mvi.t2)]) q.append(mvi.q2) t.append(mvi.t2) sys = DeltaRobotTrep(L_base, L_platform, R_base, R_platform) visual.visualize_3d([visual.VisualItem3D(sys, t, q)])
def visualize_trajectory(dsys, xid, filename=None): xi = dsys.load_state_trajectory(filename) (q, p, v, mu, rho) = dsys.split_trajectory(*xi) (qd, p, v, mu, rho) = dsys.split_trajectory(*xid) t = dsys._time t = t[:min(len(t), len(q))] q = q[:len(t)] position = [-1.695, -0.780, 0.424] angle = [0.462, 0.24, 0.0] #item1 = PuppetVisual(dsys.system, t, qd) item2 = trep.puppets.PuppetVisual(dsys.system, t, q) visual.visualize_3d([item2], camera_pos=position, camera_ang=angle)
# There is no guarantee that the configuration we just set is # consistent with the system's constraints. We can call # System.satisfy_constraints() to have trep find a configuration that # is consistent. This just uses a root-finding method to solve h(q) = # 0 starting from the current configuration. There is no guarantee # that it will always converge and no guarantee the final # configuration is close to the original one. system.satisfy_constraints() # Now we'll extract the current configuration into a tuple to use as # initial conditions for a variational integrator. q0 = system.q # Create and initialize the variational integrator mvi = trep.MidpointVI(system) mvi.initialize_from_configs(0.0, q0, dt, q0) # This is our simulation loop. We save the results in two lists. q = [mvi.q2] t = [mvi.t2] while mvi.t1 < tf: mvi.step(mvi.t2 + dt) q.append(mvi.q2) t.append(mvi.t2) # After the simulation is finished, we can visualize the results. The # viewer can automatically draw a primitive representation for # arbitrary systems from the tree structure. print_instructions() has # the viewer print out basic usage information to the console. visual.visualize_3d([visual.VisualItem3D(system, t, q)])
# Calculate an initial condition that is consistent with the constraints. system.q = 0.0 system.get_config('hel-x').q = calc_x(dt) system.satisfy_constraints() q0 = system.q # Create and initialize the variational integrator mvi = trep.MidpointVI(system) mvi.initialize_from_configs(0.0, q0, dt, q0) # This is our simulation loop. We save the results in two lists. q = [mvi.q2] t = [mvi.t2] while mvi.t1 < tf: t2 = mvi.t2 + dt mvi.step(t2, (), (calc_x(t2),)) q.append(mvi.q2) t.append(mvi.t2) # The Python constraints are much slower, so print out the time # occasionally to indicate our progress. if abs(mvi.t2 - round(mvi.t2)) < dt/2.0: print "t =",mvi.t2 # After the simulation is finished, we can visualize the results. The # viewer can automatically draw a primitive representation for # arbitrary systems from the tree structure. print_instructions() has # the viewer print out basic usage information to the console. visual.visualize_3d([visual.VisualItem3D(system, t, q)])
visual.stlmodel('./puppet-stl/torso.stl').draw) self.attachDrawing( 'Left Shoulder', visual.stlmodel('./puppet-stl/lefthumerus.stl').draw) self.attachDrawing( 'Right Shoulder', visual.stlmodel('./puppet-stl/righthumerus.stl').draw) self.attachDrawing('Left Elbow', visual.stlmodel('./puppet-stl/leftradius.stl').draw) self.attachDrawing( 'Right Elbow', visual.stlmodel('./puppet-stl/rightradius.stl').draw) self.attachDrawing('Right Hip', visual.stlmodel('./puppet-stl/femur.stl').draw) self.attachDrawing('Left Hip', visual.stlmodel('./puppet-stl/femur.stl').draw) self.attachDrawing('right Knee', visual.stlmodel('./puppet-stl/tibia.stl').draw) self.attachDrawing('Left Knee', visual.stlmodel('./puppet-stl/tibia.stl').draw) self.attachDrawing('Head', visual.stlmodel('./puppet-stl/head.stl').draw) self.attachDrawing(None, self.draw_constraints) def draw_constraints(self): for x in self.system.constraints: x.opengl_draw() visual.visualize_3d([PuppetVisual(system, t, q)])
# Right now trep stupidly requires that qk2 is a tuple. This will be changed eventually. mvi.step(mvi.t2+dt, (), tuple(qk2)) q.append(mvi.q2) t.append(mvi.t2) if abs(mvi.t2 - round(mvi.t2)) < dt/2.0: print "t =",mvi.t2 class PuppetVisual(visual.VisualItem3D): def __init__(self, *args, **kwds): super(PuppetVisual, self).__init__(*args, **kwds) self.attachDrawing('Torso', visual.stlmodel('./puppet-stl/torso.stl').draw) self.attachDrawing('Left Shoulder', visual.stlmodel('./puppet-stl/lefthumerus.stl').draw) self.attachDrawing('Right Shoulder', visual.stlmodel('./puppet-stl/righthumerus.stl').draw) self.attachDrawing('Left Elbow', visual.stlmodel('./puppet-stl/leftradius.stl').draw) self.attachDrawing('Right Elbow', visual.stlmodel('./puppet-stl/rightradius.stl').draw) self.attachDrawing('Right Hip', visual.stlmodel('./puppet-stl/femur.stl').draw) self.attachDrawing('Left Hip', visual.stlmodel('./puppet-stl/femur.stl').draw) self.attachDrawing('right Knee', visual.stlmodel('./puppet-stl/tibia.stl').draw) self.attachDrawing('Left Knee', visual.stlmodel('./puppet-stl/tibia.stl').draw) self.attachDrawing('Head', visual.stlmodel('./puppet-stl/head.stl').draw) self.attachDrawing(None, self.draw_constraints) def draw_constraints(self): for x in self.system.constraints: x.opengl_draw() visual.visualize_3d([PuppetVisual(system, t, q)])
## print f.validate_f_dq(verbose=True) ## sys.exit(1) # Create and initialize the variational integrator mvi = trep.MidpointVI(system) q0 = system.q mvi.initialize_from_configs(0.0, q0, dt, q0) # This is our simulation loop. We save the results in two lists. q = [mvi.q2] t = [mvi.t2] try: while mvi.t1 < tf: mvi.step(mvi.t2 + dt) q.append(mvi.q2) t.append(mvi.t2) except trep.ConvergenceError: print "simulation failed at %f" % mvi.t1 # We can create a custom visualization for the radial engine. class PistonVisual(visual.VisualItem3D): def __init__(self, *args, **kwds): super(PistonVisual, self).__init__(*args, **kwds) self.density = 5000 # Now we can show the simulation using the builtin basic visualizer. visual.visualize_3d([PistonVisual(system, t, q)])
# if mvi.t1 < (400*click): # u = tuple([-19*kt, 19*kt]) # else: # u = tuple([0,0]) u = tuple([0 * kt, 0 * kt]) mvi.step(mvi.t2 + dt, u, (), 5000) T.append(mvi.t1) Q.append(mvi.q1) steps = steps + 1 # while tcur < tf # stance for 100 clicks # flight # make sure tf is ~0.2 s return (T, Q) hopper_stance.q = qtrep_init # simulate print 'Simulating...' (tsim, qsim) = simulate_system(hopper_stance) print 'Done!' # visual.visualize_3d([ visual.VisualItem3D(hopper_stance, tview, qtrep_view) ], camera_pos = [(1,0,0)]) visual.visualize_3d([visual.VisualItem3D(hopper_stance, tsim, qsim)], camera_pos=[(1, 0, 0)])
def generate_cont_sim(system, q0): n = len(system.configs) def f(x, t): q = x.tolist()[:n] dq = x.tolist()[n:] system.set_state(q=q, dq=dq, ddqk=qk_dd_func(system, t), t=t) system.f() ddq = system.ddq return np.concatenate((dq, ddq)) x0 = np.concatenate((system.q, system.dq)) # Generate a list of time values we want to calculate the configuration for. t = [dt * i for i in range(int(tf / dt))] # Run the numerical integrator x = odeint(f, x0, t) # Extract the configuration out of the resulting trajectory. q = [xi.tolist()[:n] for xi in x] return t, q # t,q = generate_vi_sim(system, q0) tc, qc = generate_cont_sim(system, q0) # Display visual.visualize_3d( [visual.VisualItem3D(system, tc, qc)], camera_pos=[5.76000000e+00, -9.30731567e-17, -1.28000000e+00])
# Create and initialize the variational integrator mvi = trep.MidpointVI(system) q0 = system.q mvi.initialize_from_configs(0.0, q0, dt, q0) # This is our simulation loop. We save the results in two lists. q = [mvi.q2] t = [mvi.t2] try: while mvi.t1 < tf: mvi.step(mvi.t2+dt) q.append(mvi.q2) t.append(mvi.t2) except trep.ConvergenceError: print "simulation failed at %f" % mvi.t1 # We can create a custom visualization for the radial engine. class PistonVisual(visual.VisualItem3D): def __init__(self, *args, **kwds): super(PistonVisual, self).__init__(*args, **kwds) self.density = 5000 # Now we can show the simulation using the builtin basic visualizer. visual.visualize_3d([PistonVisual(system, t, q)])