forked from dfki-ric-underactuated-lab/double_pendulum
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathdircol_drake2.py
122 lines (106 loc) · 3.32 KB
/
dircol_drake2.py
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
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
import time
from datetime import datetime
import os
import numpy as np
from double_pendulum.trajectory_optimization.direct_collocation.direct_collocation import DirCol
from double_pendulum.utils.plotting import plot_timeseries
from double_pendulum.utils.csv_trajectory import save_trajectory, load_trajectory
from double_pendulum.model.symbolic_plant import SymbolicDoublePendulum
from double_pendulum.model.model_parameters import model_parameters
from double_pendulum.simulation.simulation import Simulator
from double_pendulum.controller.trajectory_following.trajectory_controller import TrajectoryController
# model parameters
design = "design_A.0"
model = "model_1.2"
robot = "acrobot"
urdf_path = "../../data/urdfs/design_A.0/model_1.0/"+robot+".urdf"
torque_limit = 6.0
mpar = model_parameters(
torque_limit = [torque_limit]*2,
model_design=design,
model_id=model,
robot=robot
)
# Trajectory parameters
initial_state = np.array([0.0, 0.0, 0., 0.])
final_state = np.array([np.pi, 0.0, 0.0, 0.0])
n = 20
init_traj_time_interval = [0., 10.]
freq = 1000
# limits
theta_limit = float(np.deg2rad(360.))
speed_limit = 10
minimum_timestep = 0.01
maximum_timestep = 0.2
# Initial guesses
X_initial = np.zeros((4,n))
X_initial[0,:] = np.linspace(start=0,stop=np.pi, num=n)
U_initial = np.zeros((2,n))
h_initial = maximum_timestep
# costs
R = np.eye(2) * 0.01
Q = np.diag([1,1,20,20])
wh = 20
# saving
timestamp = datetime.today().strftime("%Y%m%d-%H%M%S")
save_dir = os.path.join("data/trajectories", design, model, robot, "dircol", "trajopt", timestamp)
os.makedirs(save_dir)
# Direct Collocation calculation
t0 = time.time()
dc = DirCol(urdf_path,
robot,
modelPars=mpar,
saveDir=save_dir)
dc.MathematicalProgram(
N=n,
wh=wh,
R=R,
Q=Q,
h_min=minimum_timestep,
h_max=maximum_timestep,
x0=initial_state,
xf=final_state,
torque_limit=torque_limit,
X_initial=X_initial,
U_initial=U_initial,
h_initial=h_initial
)
T, X, U = dc.ComputeTrajectory(freq=freq)
print("Computing time: ", time.time() - t0, "s")
traj_file = os.path.join(save_dir, "trajectory.csv")
save_trajectory(csv_path=traj_file,
T=T, X=X, U=U)
# plotting
plot_timeseries(T, X, U, None,
plot_energy=False,
pos_y_lines=[0.0, np.pi],
tau_y_lines=[-torque_limit, torque_limit],
save_to=os.path.join(save_dir, "timeseries"))
## animate TODO: currently not supported
## animation with meshcat in browser window
#print("animating...")
#dc.animate_trajectory()
#
## simulate in python plant
#dt = T[1] - T[0]
#t_final = T[-1]
#x0 = X[0]
#
#plant = SymbolicDoublePendulum(model_pars=mpar)
#sim = Simulator(plant=plant)
#
#controller = TrajectoryController(csv_path=traj_file,
# torque_limit=torque_limit,
# kK_stabilization=False)
#controller.init()
#
#T, X, U = sim.simulate_and_animate(t0=0.0, x0=x0,
# tf=t_final, dt=dt, controller=controller,
# integrator="runge_kutta",
# plot_inittraj=True)
#
#plot_timeseries(T, X, U, None,
# plot_energy=False,
# pos_y_lines=[0.0, np.pi],
# tau_y_lines=[-torque_limit_active, torque_limit_active])
#print("done")