-
Notifications
You must be signed in to change notification settings - Fork 4
/
run_acrocart
executable file
·43 lines (37 loc) · 1.33 KB
/
run_acrocart
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
#!/usr/bin/python
"""
Script to run the acrocart demo.
"""
from __future__ import division
import numpy as np; npl = np.linalg
from scipy.interpolate import interp1d
import acrocart
import time
# Set-up dynamics and optimal controller
dyn = acrocart.Dynamics()
opt = acrocart.Optimizer(dyn, verbosity=5)
# Frame problem
q0 = [0, 0, 0, 0, 0, 0]
qN = [1, np.pi, np.pi, 0, 0, 0] #??? since this solver doesnt do angle error on SO2, pi and -pi are different
tN = 4
dt = 0.01
goal = qN[0]
H = np.array((50, 5, 1)) * dt
rush = 0
# Get optimal control
opt_start_time = time.time()
Topt, Uopt, Qopt, opt_success = opt.make_trajectory(q0, qN, tN, H, rush=rush)
control = opt.make_controller(Topt, Uopt, Qopt)
if not opt_success: print "OPTIMIZATION FAILED"
else: print "OPTIMIZATION SUCCEEDED"
print "TOTAL TIME TAKEN BY OPTIMIZATION: {}".format(np.round(time.time() - opt_start_time, 3))
print "----------------------------------------\n"
# Look at ideal plan or simulate its execution?
override = None # True or None
if override:
print "(visualizing ideal scenario)"
override = interp1d(Topt, Qopt, kind="quadratic", assume_sorted=True,
axis=0, bounds_error=False, fill_value=Qopt[-1])
# Set-up simulator and run
sim = acrocart.Simulator(dyn)
sim.simulate(q0, control, tN=0, dt=dt, goal=goal, override=override, Des=(Topt, Qopt))