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