test_dynamic_time_indexed_shooting_problem.py
Go to the documentation of this file.
1 # coding: utf-8
2 from __future__ import print_function, division
3 import roslib
4 import unittest
5 import numpy as np
6 import pyexotica as exo
7 from pyexotica.testing import random_state
8 import sys
9 
10 PKG = 'exotica_examples'
11 roslib.load_manifest(PKG) # This line is not needed with Catkin.
12 
13 # np.random.seed(42)
14 np.set_printoptions(2, threshold=sys.maxsize, suppress=True, linewidth=500)
15 
16 def check_state_cost_jacobian_at_t(problem, t):
17  scene = problem.get_scene()
18  ds = scene.get_dynamics_solver()
19 
20  x = random_state(ds)
21  u = np.random.random((ds.nu,))
22 
23  if t == problem.T - 1:
24  problem.update_terminal_state(x)
25  else:
26  problem.update(x, u, t)
27  J_solver = problem.get_state_cost_jacobian(t).copy()
28  J_numdiff = np.zeros_like(J_solver)
29  np.testing.assert_equal(J_numdiff.shape[0], ds.ndx)
30 
31  original_integrator = ds.integrator
32  eps = 1e-6
33  for i in range(ds.ndx):
34  dx = np.zeros((ds.ndx,))
35  dx[i] = eps / 2.0
36 
37  ds.integrator = exo.Integrator.RK1
38  x_plus = ds.integrate(x, dx, 1.0)
39  x_minus = ds.integrate(x, -dx, 1.0)
40  ds.integrator = original_integrator
41 
42  if t == problem.T - 1:
43  problem.update_terminal_state(x_plus)
44  else:
45  problem.update(x_plus, u, t)
46  cost_high = problem.get_state_cost(t)
47 
48  if t == problem.T - 1:
49  problem.update_terminal_state(x_minus)
50  else:
51  problem.update(x_minus, u, t)
52  cost_low = problem.get_state_cost(t)
53 
54  J_numdiff[i] = (cost_high - cost_low) / eps
55  np.testing.assert_allclose(J_solver, J_numdiff, rtol=3e-5,
56  atol=1e-3, err_msg='StateCostJacobian does not match!')
57 
58 
59 def check_state_cost_hessian_at_t(problem, t):
60  scene = problem.get_scene()
61  ds = scene.get_dynamics_solver()
62 
63  x = random_state(ds)
64  u = np.random.random((ds.nu,))
65 
66  if t == problem.T - 1:
67  problem.update_terminal_state(x)
68  else:
69  problem.update(x, u, t)
70  H_solver = problem.get_state_cost_hessian(t).copy()
71  H_numdiff = np.zeros_like(H_solver)
72  np.testing.assert_equal(H_numdiff.shape[0], ds.ndx)
73  np.testing.assert_equal(H_numdiff.shape[1], ds.ndx)
74 
75  original_integrator = ds.integrator
76  eps = 1e-6
77  for i in range(ds.ndx):
78  dx = np.zeros((ds.ndx,))
79  dx[i] = eps / 2.0
80 
81  ds.integrator = exo.Integrator.RK1
82  x_plus = ds.integrate(x, dx, 1.0)
83  x_minus = ds.integrate(x, -dx, 1.0)
84  ds.integrator = original_integrator
85 
86  if t == problem.T - 1:
87  problem.update_terminal_state(x_plus)
88  else:
89  problem.update(x_plus, u, t)
90  jacobian_plus = problem.get_state_cost_jacobian(t)
91 
92  if t == problem.T - 1:
93  problem.update_terminal_state(x_minus)
94  else:
95  problem.update(x_minus, u, t)
96  jacobian_minus = problem.get_state_cost_jacobian(t)
97 
98  H_numdiff[:,i] = (jacobian_plus - jacobian_minus) / eps
99  if np.linalg.norm(H_solver - H_numdiff) > 1e-2:
100  print("Hessian does not match at t=", t)
101  print(H_solver)
102  print(H_numdiff)
103  print((H_solver-H_numdiff)<1e-3)
104  print(H_solver-H_numdiff)
105  np.testing.assert_allclose(H_solver, H_numdiff, rtol=1e-5,
106  atol=1e-2, err_msg='StateCostHessian does not match!')
107 
108 
109 def check_control_cost_jacobian_at_t(problem, t):
110  scene = problem.get_scene()
111  ds = scene.get_dynamics_solver()
112 
113  x = random_state(ds)
114  u = np.random.random((ds.nu,))
115 
116  problem.update(x, u, t)
117  J_solver = problem.get_control_cost_jacobian(t).copy()
118  J_numdiff = np.zeros_like(J_solver)
119  np.testing.assert_equal(J_numdiff.shape[0], ds.nu)
120 
121  eps = 1e-6
122  for i in range(ds.nu):
123  u_plus = u.copy()
124  u_plus[i] += eps / 2.0
125  u_minus = u.copy()
126  u_minus[i] -= eps / 2.0
127 
128  problem.update(x, u_plus, t)
129  cost_high = problem.get_control_cost(t)
130 
131  problem.update(x, u_minus, t)
132  cost_low = problem.get_control_cost(t)
133 
134  J_numdiff[i] = (cost_high - cost_low) / eps
135  np.testing.assert_allclose(J_solver, J_numdiff, rtol=1e-5,
136  atol=1e-5, err_msg='ControlCostJacobian does not match!')
137 
138 
139 def check_control_cost_hessian_at_t(problem, t):
140  scene = problem.get_scene()
141  ds = scene.get_dynamics_solver()
142 
143  x = random_state(ds)
144  u = np.random.random((ds.nu,))
145 
146  problem.update(x, u, t)
147  H_solver = problem.get_control_cost_hessian(t).copy()
148  H_numdiff = np.zeros_like(H_solver)
149  np.testing.assert_equal(H_numdiff.shape[0], ds.nu)
150  np.testing.assert_equal(H_numdiff.shape[1], ds.nu)
151 
152  eps = 1e-6
153  for i in range(ds.nu):
154  u_plus = u.copy()
155  u_plus[i] += eps / 2.0
156  u_minus = u.copy()
157  u_minus[i] -= eps / 2.0
158 
159  problem.update(x, u_plus, t)
160  jacobian_plus = problem.get_control_cost_jacobian(t)
161 
162  problem.update(x, u_minus, t)
163  jacobian_minus = problem.get_control_cost_jacobian(t)
164 
165  H_numdiff[:,i] = (jacobian_plus - jacobian_minus) / eps
166  np.testing.assert_allclose(H_solver, H_numdiff, rtol=1e-5,
167  atol=1e-5, err_msg='ControlCostHessian does not match!')
168 
169 ###############################################################################
170 
171 if __name__ == "__main__":
172  # Load a problem
173  configs = [
174  '{exotica_examples}/resources/configs/dynamic_time_indexed/01_ilqr_cartpole.xml',
175  '{exotica_examples}/resources/configs/dynamic_time_indexed/02_lwr_task_maps.xml',
176  '{exotica_examples}/resources/configs/dynamic_time_indexed/03_ilqr_valkyrie.xml',
177  '{exotica_examples}/resources/configs/dynamic_time_indexed/04_analytic_ddp_cartpole.xml',
178  '{exotica_examples}/resources/configs/dynamic_time_indexed/05_analytic_ddp_lwr.xml',
179  '{exotica_examples}/resources/configs/dynamic_time_indexed/06_analytic_ddp_valkyrie.xml',
180  '{exotica_examples}/resources/configs/dynamic_time_indexed/07_control_limited_ddp_cartpole.xml',
181  '{exotica_examples}/resources/configs/dynamic_time_indexed/08_control_limited_ddp_lwr.xml',
182  '{exotica_examples}/resources/configs/dynamic_time_indexed/09_control_limited_ddp_valkyrie.xml',
183  '{exotica_examples}/resources/configs/dynamic_time_indexed/10_ilqg_cartpole.xml',
184  '{exotica_examples}/resources/configs/dynamic_time_indexed/11_ilqg_lwr.xml',
185  '{exotica_examples}/resources/configs/dynamic_time_indexed/12_ilqg_valkyrie.xml',
186  '{exotica_examples}/resources/configs/dynamic_time_indexed/13_control_limited_ddp_quadrotor.xml',
187  '{exotica_examples}/resources/configs/dynamic_time_indexed/14_rrt_cartpole.xml',
188  '{exotica_examples}/resources/configs/dynamic_time_indexed/15_rrt_quadrotor.xml',
189  '{exotica_examples}/resources/configs/dynamic_time_indexed/16_kpiece_cartpole.xml',
190  '{exotica_examples}/resources/configs/dynamic_time_indexed/17_quadrotor_collision_avoidance.xml',
191  '{exotica_examples}/resources/configs/dynamic_time_indexed/18_ilqr_pendulum.xml',
192  '{exotica_examples}/resources/configs/dynamic_time_indexed/19_ddp_quadrotor_sphere.xml',
193  '{exotica_examples}/resources/configs/dynamic_time_indexed/20_sparse_fddp_pendulum.xml',
194  '{exotica_examples}/resources/configs/dynamic_time_indexed/21_sparse_fddp_cartpole.xml',
195  '{exotica_examples}/resources/configs/dynamic_time_indexed/22_boxfddp_cartpole.xml',
196  ]
197 
198  for config in configs:
199  print("Testing", config)
200  _, problem_init = exo.Initializers.load_xml_full(config)
201  problem = exo.Setup.create_problem(problem_init)
202  scene = problem.get_scene()
203  ds = problem.get_scene().get_dynamics_solver()
204 
205  # print(problem, ds)
206  # print(ds.nq, ds.nv, ds.nx, ds.ndx)
207 
208  # test state cost jacobian
209  for t in range(problem.T):
210  check_state_cost_jacobian_at_t(problem, t)
211 
212  # TODO: test state cost hessian
213  # Deactivated as part of it will be a Gauss-Newton approximation
214  for t in range(problem.T):
215  check_state_cost_hessian_at_t(problem, t)
216 
217  # test control cost jacobian
218  for t in range(problem.T - 1):
219  check_control_cost_jacobian_at_t(problem, t)
220 
221  # test control cost hessian
222  for t in range(problem.T - 1):
223  check_control_cost_hessian_at_t(problem, t)
224 
225  # TODO: test state control cost hessian
226  # We assume this to be 0.
test.test_dynamic_time_indexed_shooting_problem.check_control_cost_hessian_at_t
def check_control_cost_hessian_at_t(problem, t)
Definition: test_dynamic_time_indexed_shooting_problem.py:139
test.test_dynamic_time_indexed_shooting_problem.check_state_cost_jacobian_at_t
def check_state_cost_jacobian_at_t(problem, t)
Definition: test_dynamic_time_indexed_shooting_problem.py:16
test.test_dynamic_time_indexed_shooting_problem.check_control_cost_jacobian_at_t
def check_control_cost_jacobian_at_t(problem, t)
Definition: test_dynamic_time_indexed_shooting_problem.py:109
test.test_dynamic_time_indexed_shooting_problem.check_state_cost_hessian_at_t
def check_state_cost_hessian_at_t(problem, t)
Definition: test_dynamic_time_indexed_shooting_problem.py:59

exotica_examples
Author(s):
autogenerated on Sat Apr 10 2021 02:37:17