testing.py
Go to the documentation of this file.
1 # coding: utf-8
2 from __future__ import print_function, division
3 
4 import numpy as np
5 import pyexotica as exo
6 import sys
7 
8 __all__ = ["check_dynamics_solver_derivatives"]
9 
10 # np.random.seed(42)
11 np.set_printoptions(4, suppress=True, threshold=sys.maxsize, linewidth=500)
12 
14  # Using http://planning.cs.uiuc.edu/node198.html
15  uvw = np.random.uniform(size=(3,))
16  return np.array([np.sqrt(1.0-uvw[0]) * np.sin(2.0*np.pi*uvw[1]),
17  np.sqrt(1.0-uvw[0]) * np.cos(2.0*np.pi*uvw[1]),
18  np.sqrt(uvw[0])*np.sin(2.0*np.pi*uvw[2]),
19  np.sqrt(uvw[0])*np.cos(2.0*np.pi*uvw[2])])
20 
21 
22 def random_state(ds):
23  x = np.random.random((ds.nx,))
24 
25  # Use random quaternion when floating base is represented using SE(3)
26  # and scene.get_kinematic_tree().get_model_base_type() == exo.BaseType.Floating:
27  if ds.ndx != ds.nq + ds.nv:
28  x[3:7] = random_quaternion()
29  x[3:7] /= np.linalg.norm(x[3:7])
30 
31  return x
32 
33 
34 def explicit_euler(x, dx, dt, ds=None):
35  return x + dt * dx
36 
37 
38 def semiimplicit_euler(x, dx, dt, ds=None):
39  if ds is None:
40  raise RuntimeError("ds is None!")
41  v = x[ds.nq:].copy()
42  a = dx[ds.nq:].copy()
43 
44  dx_new = np.concatenate([dt * v + (dt*dt) * a, dt * a])
45  return x + dx_new
46 
47 
48 def check_dynamics_solver_derivatives(name, urdf=None, srdf=None, joint_group=None, additional_args=None, do_test_integrators=True):
49  ds = None
50  if urdf is not None and srdf is not None and joint_group is not None:
51  my_scene_init = exo.Initializers.SceneInitializer()
52  my_scene_init[1]['URDF'] = urdf
53  my_scene_init[1]['SRDF'] = srdf
54  my_scene_init[1]['JointGroup'] = joint_group
55  my_scene_init[1]['DynamicsSolver'] = [(name, {'Name': u'MyDynamicsSolver'})]
56  if additional_args is not None:
57  my_scene_init[1]['DynamicsSolver'][0][1].update(additional_args)
58  scene = exo.Setup.create_scene(exo.Initializers.Initializer(my_scene_init))
59  ds = scene.get_dynamics_solver()
60  else:
61  my_ds_init = (name, {'Name': u'MyDynamicsSolver'})
62  if additional_args is not None:
63  my_ds_init.merge(additional_args)
64  ds = exo.Setup.create_dynamics_solver(my_ds_init)
65 
66  # Get random state, control
67  x = random_state(ds)
68  u = np.random.random((ds.nu,))
69 
70  # f should return tangent vector type
71  np.testing.assert_equal(ds.f(x,u).shape[0], ds.ndx)
72  # fx should be (ds.ndx,ds.ndx)
73  np.testing.assert_equal(ds.fx(x,u).shape[0], ds.ndx)
74  np.testing.assert_equal(ds.fx(x,u).shape[1], ds.ndx)
75  # fu should be (ds.ndx,ds.nu)
76  np.testing.assert_equal(ds.fu(x,u).shape[0], ds.ndx)
77  np.testing.assert_equal(ds.fu(x,u).shape[1], ds.nu)
78 
79  # Check integration / simulate
80  dx = ds.f(x,u)
81  np.testing.assert_array_equal(ds.simulate(x, u, 0.01), ds.integrate(x, dx, 0.01))
82 
83  # Checking finite difference derivatives
84 
85  ## fu
86  fu = ds.fu(x,u)
87  fu_fd = ds.fu_fd(x,u)
88  # if np.linalg.norm(fu-fu_fd) > 1e-3 or np.any(np.isnan(fu)):
89  # print(fu-fu_fd<1e-3)
90  # print(fu-fu_fd)
91  # print("fu\n",fu)
92  # print("fu_fd\n",fu_fd)
93  np.testing.assert_allclose(fu, fu_fd, rtol=1e-5, atol=1e-5)
94 
95  ## fx
96  fx = ds.fx(x,u)
97  fx_fd = ds.fx_fd(x,u)
98  if np.linalg.norm(fx-fx_fd) > 1e-3 or np.any(np.isnan(fx)):
99  print(fx-fx_fd<1e-3)
100  fx_fd[fx_fd<1e-6] = 0.
101  print(fx-fx_fd, 2)
102  print("fx\n",fx)
103  print("fx_fd\n",fx_fd)
104  np.testing.assert_allclose(fx, fx_fd, rtol=1e-5, atol=1e-5, err_msg='fx does not match!')
105 
106  # Check joint computation
107  ds.compute_derivatives(x, u)
108  fx_joint = ds.get_fx()
109  fu_joint = ds.get_fu()
110  np.testing.assert_allclose(fx, fx_joint, rtol=1e-5, atol=1e-5)
111  np.testing.assert_allclose(fu, fu_joint, rtol=1e-5, atol=1e-5)
112 
113  # Check different integration schemes
114  if ds.nq == ds.nv and do_test_integrators:
115  python_integrators = {
116  exo.Integrator.RK1: explicit_euler,
117  exo.Integrator.SymplecticEuler: semiimplicit_euler
118  }
119  for integrator in [exo.Integrator.RK1, exo.Integrator.SymplecticEuler]:
120  ds.integrator = integrator
121  for dt in [0.001, 0.01, 1.0]:
122  print("Testing integrator", integrator, "dt=", dt)
123  np.testing.assert_allclose(ds.integrate(
124  x, dx, dt), python_integrators[integrator](x, dx, dt, ds))
125 
126  # Check state transition function and its derivative for each integration scheme
127  for integrator in [exo.Integrator.RK1, exo.Integrator.SymplecticEuler]:
128  print("Testing state transition for", integrator, "dt=", ds.dt)
129  ds.integrator = integrator
130  eps = 1e-5
131  Fx_fd = np.zeros((ds.ndx, ds.ndx))
132  for i in range(ds.ndx):
133  dx = np.zeros((ds.ndx))
134  dx[i] = eps / 2.0
135  # For finite-diff, we need to use RK1 to compute x_plus, x_minus
136  ds.integrator = exo.Integrator.RK1
137  x_plus = ds.integrate(x, dx, 1.0)
138  x_minus = ds.integrate(x, -dx, 1.0)
139  ds.integrator = integrator
140  F_plus = ds.F(x_plus, u)
141  F_minus = ds.F(x_minus, u)
142  Fx_fd[:,i] = ds.state_delta(F_plus, F_minus) / eps
143 
144  Fu_fd = np.zeros((ds.ndx, ds.nu))
145  for i in range(ds.nu):
146  du = np.zeros((ds.nu))
147  du[i] = eps / 2.0
148  u_plus = u + du
149  u_minus = u - du
150  F_plus = ds.F(x, u_plus)
151  F_minus = ds.F(x, u_minus)
152  Fu_fd[:,i] = ds.state_delta(F_plus, F_minus) / eps
153 
154  # Run several times to catch the "adding rather than resetting bug"
155  ds.compute_derivatives(x, u)
156  ds.compute_derivatives(x, u)
157  ds.compute_derivatives(x, u)
158 
159  # print("Fx_fd\n", Fx_fd)
160  # print("ds.get_Fx()\n", ds.get_Fx())
161  # print("Fx-diff\n", (Fx_fd-ds.get_Fx()))
162  # print(np.abs(Fx_fd-ds.get_Fx()) < 1e-5)
163  # print("Fu_fd\n", Fu_fd)
164  # print("ds.get_Fu()\n", ds.get_Fu())
165  # print("Fu-diff\n", (Fu_fd-ds.get_Fu()))
166  # print(np.abs(Fu_fd-ds.get_Fu()) < 1e-5)
167 
168  np.testing.assert_allclose(ds.get_Fx(), Fx_fd, rtol=1e-5, atol=1e-5)
169  np.testing.assert_allclose(ds.get_Fu(), Fu_fd, rtol=1e-5, atol=1e-5)
170 
171  # Check state delta and its derivatives
172  if ds.nq != ds.nv:
173  #print("Non-Euclidean space: Check StateDelta")
174  x1 = random_state(ds)
175  x2 = random_state(ds)
176 
177  # Check dStateDelta
178  Jds = ds.state_delta_derivative(x1, x2, exo.ArgumentPosition.ARG0)
179  Jdiff = np.zeros((ds.ndx, ds.ndx))
180  eps = 1e-5
181  integrator = ds.integrator
182  for i in range(ds.ndx):
183  dx = np.zeros((ds.ndx))
184  dx[i] = eps / 2.0
185  ds.integrator = exo.Integrator.RK1
186  x1_plus = ds.integrate(x1, dx, 1.0)
187  x1_minus = ds.integrate(x1, -dx, 1.0)
188  ds.integrator = integrator
189  delta_plus = ds.state_delta(x1_plus, x2)
190  delta_minus = ds.state_delta(x1_minus, x2)
191  Jdiff[:,i] = (delta_plus - delta_minus) / eps
192  np.testing.assert_allclose(Jds, Jdiff, rtol=1e-5, atol=1e-5)
193 
194  # Check ddStateDelta
195  # TODO: Verify
196  x1 = random_state(ds)
197  x2 = random_state(ds)
198  Hds = ds.state_delta_second_derivative(x1, x2, exo.ArgumentPosition.ARG0)
199  Hdiff = np.zeros((ds.ndx, ds.ndx, ds.ndx))
200  eps = 1e-5
201  integrator = ds.integrator
202  for i in range(ds.ndx):
203  dx = np.zeros((ds.ndx))
204  dx[i] = eps / 2.0
205  ds.integrator = exo.Integrator.RK1
206  x1_plus = ds.integrate(x1, dx, 1.0)
207  x1_minus = ds.integrate(x1, -dx, 1.0)
208  ds.integrator = integrator
209  Jdiff_plus = ds.state_delta_derivative(x1_plus, x2, exo.ArgumentPosition.ARG0)
210  Jdiff_minus = ds.state_delta_derivative(x1_minus, x2, exo.ArgumentPosition.ARG0)
211  Hdiff[:,:,i] = (Jdiff_plus - Jdiff_minus) / eps
212  np.testing.assert_allclose(Hds, Hdiff, rtol=1e-5, atol=1e-5)
def explicit_euler(x, dx, dt, ds=None)
Definition: testing.py:34
def semiimplicit_euler(x, dx, dt, ds=None)
Definition: testing.py:38
def check_dynamics_solver_derivatives(name, urdf=None, srdf=None, joint_group=None, additional_args=None, do_test_integrators=True)
Definition: testing.py:48
def random_state(ds)
Definition: testing.py:22
def random_quaternion()
Definition: testing.py:13


exotica_python
Author(s):
autogenerated on Mon Feb 22 2021 03:33:27