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)])
