test_problems.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018, University of Edinburgh
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without specific
15 // prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
30 #include <exotica_core/exotica_core.h>
31 #include <exotica_core/tools/test_helpers.h>
32 #include <gtest/gtest.h>
33 
34 using namespace exotica;
35 #include <string>
36 #include <vector>
37 
38 #define CREATE_PROBLEM(X, I) std::shared_ptr<X> problem = CreateProblem<X>(#X, I);
39 #define NUM_TRIALS 100
40 
41 // workaround for "error: ambiguous overload for ‘operator-’" that appear in newer Eigen versions
42 #if EIGEN_VERSION_AT_LEAST(3, 3, 0)
43 Hessian operator-(const Hessian& A, const Hessian& B)
44 {
45  if (A.rows() != B.rows())
46  {
47  throw std::runtime_error("Hessian dimension mismatch");
48  }
49 
50  Hessian ret(A.rows());
51  for (int i = 0; i < A.rows(); ++i)
52  {
53  ret[i] = A[i] - B[i];
54  }
55  return ret;
56 }
57 #endif
58 
59 template <class T>
60 std::shared_ptr<T> CreateProblem(const std::string& name, int derivative)
61 {
62  TEST_COUT << "Creating " << name << " with derivatives " << derivative;
63  Initializer dummy;
64  Initializer init;
65  XMLLoader::Load("{exotica_examples}/test/resources/test_problems.xml", dummy, init, "Dummy", name);
66  init.AddProperty(Property("DerivativeOrder", false, derivative));
67  std::shared_ptr<T> ret = std::static_pointer_cast<T>(Setup::CreateProblem(init));
68  TEST_COUT << "Problem loaded";
69  return ret;
70 }
71 
72 template <class T>
73 void testJacobianEndPose(std::shared_ptr<T> problem, EndPoseTask& task, double eps = 1e-4, double h = 1e-5)
74 {
75  TEST_COUT << "Testing Jacobian:";
76  for (int tr = 0; tr < NUM_TRIALS; ++tr)
77  {
78  Eigen::VectorXd x0(problem->N);
79  x0.setRandom();
80  problem->Update(x0);
81  TaskSpaceVector y0 = task.Phi;
82  Eigen::MatrixXd J0 = task.jacobian;
83  Eigen::MatrixXd J = Eigen::MatrixXd::Zero(J0.rows(), J0.cols());
84  for (int i = 0; i < problem->N; ++i)
85  {
86  Eigen::VectorXd x = x0;
87  x(i) += h;
88  problem->Update(x);
89  J.col(i) = (task.Phi - y0) / h;
90  }
91  double errJ = (J - J0).norm();
92  if (errJ > eps)
93  {
94  TEST_COUT << "x: " << x0.transpose();
95  TEST_COUT << "J*:\n"
96  << J;
97  TEST_COUT << "J:\n"
98  << J0;
99  ADD_FAILURE() << "Jacobian error out of bounds: " << errJ;
100  }
101  }
102  TEST_COUT << "Test passed";
103 }
104 
105 template <class T>
106 void testHessianEndPose(std::shared_ptr<T> problem, EndPoseTask& task, double eps = 1e-4, double h = 1e-5)
107 {
108  TEST_COUT << "Testing Hessian:";
109  for (int tr = 0; tr < NUM_TRIALS; ++tr)
110  {
111  Eigen::VectorXd x0(problem->N);
112  x0.setRandom();
113  problem->Update(x0);
114 
115  Hessian H_task = task.hessian;
116 
117  Hessian H_fd = Hessian::Constant(problem->length_jacobian, Eigen::MatrixXd::Zero(problem->N, problem->N));
118  Eigen::VectorXd x(problem->N);
119  for (int j = 0; j < problem->N; ++j)
120  {
121  x = x0;
122  x(j) += h;
123  problem->Update(x);
124  const Eigen::MatrixXd J1 = problem->jacobian;
125  x = x0;
126  x(j) -= h;
127  problem->Update(x);
128  const Eigen::MatrixXd J2 = problem->jacobian;
129  for (int i = 0; i < problem->N; ++i)
130  {
131  for (int k = 0; k < problem->length_jacobian; ++k)
132  {
133  H_fd(k)(i, j) = (J1(k, i) - J2(k, i)) / (2.0 * h);
134  }
135  }
136  }
137  double errH = 0;
138  for (int i = 0; i < H_fd.rows(); ++i) errH += (H_fd(i) - H_task(i)).norm();
139 
140  Hessian dH = H_fd - H_task;
141  if (errH > eps)
142  {
143  for (int i = 0; i < dH.rows(); ++i)
144  {
145  TEST_COUT << "Computed:\n"
146  << H_task(i);
147  TEST_COUT << "FD:\n"
148  << H_fd(i);
149  TEST_COUT << "Diff:\n"
150  << dH(i);
151  }
152  ADD_FAILURE() << "Hessian error out of bounds: " << errH;
153  }
154  }
155  TEST_COUT << "Test passed";
156 }
157 
158 template <class T>
159 void testJacobianTimeIndexed(std::shared_ptr<T> problem, TimeIndexedTask& task, int t, double eps = 1e-4, double h = 1e-5)
160 {
161  TEST_COUT << "Testing Jacobian:";
162  for (int tr = 0; tr < NUM_TRIALS; ++tr)
163  {
164  Eigen::VectorXd x0(problem->N);
165  x0.setRandom();
166  problem->Update(x0, t);
167  TaskSpaceVector y0 = task.Phi[t];
168  Eigen::MatrixXd J0 = task.jacobian[t];
169  Eigen::MatrixXd J = Eigen::MatrixXd::Zero(J0.rows(), J0.cols());
170  for (int i = 0; i < problem->N; ++i)
171  {
172  Eigen::VectorXd x = x0;
173  x(i) += h;
174  problem->Update(x, t);
175  J.col(i) = (task.Phi[t] - y0) / h;
176  }
177  double errJ = (J - J0).norm();
178  if (errJ > eps)
179  {
180  TEST_COUT << "x: " << x0.transpose();
181  TEST_COUT << "J*:\n"
182  << J;
183  TEST_COUT << "J:\n"
184  << J0;
185  ADD_FAILURE() << "Jacobian error out of bounds: " << errJ;
186  }
187  }
188  TEST_COUT << "Test passed";
189 }
190 
191 template <class T>
192 void testHessianTimeIndexed(std::shared_ptr<T> problem, TimeIndexedTask& task, int t, double eps = 1e-4, double h = 1e-5)
193 {
194  TEST_COUT << "Testing Hessian:";
195  for (int tr = 0; tr < NUM_TRIALS; ++tr)
196  {
197  Eigen::VectorXd x0(problem->N);
198  x0.setRandom();
199  problem->Update(x0, t);
200 
201  Hessian H_task = task.hessian[t];
202 
203  Hessian H_fd = Hessian::Constant(problem->length_jacobian, Eigen::MatrixXd::Zero(problem->N, problem->N));
204  Eigen::VectorXd x(problem->N);
205  for (int j = 0; j < problem->N; ++j)
206  {
207  x = x0;
208  x(j) += h;
209  problem->Update(x, t);
210  const Eigen::MatrixXd J1 = problem->jacobian[t];
211  x = x0;
212  x(j) -= h;
213  problem->Update(x, t);
214  const Eigen::MatrixXd J2 = problem->jacobian[t];
215  for (int i = 0; i < problem->N; ++i)
216  {
217  for (int k = 0; k < problem->length_jacobian; ++k)
218  {
219  H_fd(k)(i, j) = (J1(k, i) - J2(k, i)) / (2.0 * h);
220  }
221  }
222  }
223  double errH = 0;
224  for (int i = 0; i < H_fd.rows(); ++i) errH += (H_fd(i) - H_task(i)).norm();
225 
226  Hessian dH = H_fd - H_task;
227  if (errH > eps)
228  {
229  for (int i = 0; i < dH.rows(); ++i)
230  {
231  TEST_COUT << "Computed:\n"
232  << H_task(i);
233  TEST_COUT << "FD:\n"
234  << H_fd(i);
235  TEST_COUT << "Diff:\n"
236  << dH(i);
237  }
238  ADD_FAILURE() << "Hessian error out of bounds: " << errH;
239  }
240  }
241  TEST_COUT << "Test passed";
242 }
243 
244 TEST(ExoticaProblems, UnconstrainedEndPoseProblem)
245 {
246  try
247  {
248  std::vector<Eigen::VectorXd> X(3);
249  std::vector<Eigen::MatrixXd> J(3);
250  for (int d = 0; d < 3; ++d)
251  {
252  CREATE_PROBLEM(UnconstrainedEndPoseProblem, d);
253  Eigen::VectorXd x = problem->GetStartState();
254  TEST_COUT << "Testing problem update";
255  problem->Update(x);
256  TEST_COUT << "Test passed";
257  X[d] = problem->cost.ydiff;
258  TEST_COUT << "Testing cost";
259  if (d > 0)
260  {
261  J[d] = problem->cost.jacobian;
262  testJacobianEndPose(problem, problem->cost);
263  if (d > 1)
264  {
265  testHessianEndPose(problem, problem->cost);
266  }
267  }
268  }
269  if (!(X[0] == X[1] && X[1] == X[2]))
270  ADD_FAILURE() << "Cost FK is inconsistent!";
271  if (!(J[1] == J[2]))
272  ADD_FAILURE() << "Cost Jacobians are inconsistent!";
273  }
274  catch (const std::exception& e)
275  {
276  ADD_FAILURE() << "Uncaught exception! " << e.what();
277  }
278 }
279 
280 TEST(ExoticaProblems, BoundedEndPoseProblem)
281 {
282  try
283  {
284  std::vector<Eigen::VectorXd> X(3);
285  std::vector<Eigen::MatrixXd> J(3);
286  for (int d = 0; d < 3; ++d)
287  {
288  CREATE_PROBLEM(BoundedEndPoseProblem, d);
289  Eigen::VectorXd x = problem->GetStartState();
290  TEST_COUT << "Testing problem update";
291  problem->Update(x);
292  TEST_COUT << "Test passed";
293  X[d] = problem->cost.ydiff;
294  TEST_COUT << "Testing cost";
295  if (d > 0)
296  {
297  J[d] = problem->cost.jacobian;
298  testJacobianEndPose(problem, problem->cost);
299  if (d > 1)
300  {
301  testHessianEndPose(problem, problem->cost);
302  }
303  }
304  }
305  if (!(X[0] == X[1] && X[1] == X[2]))
306  ADD_FAILURE() << "Cost FK is inconsistent!";
307  if (!(J[1] == J[2]))
308  ADD_FAILURE() << "Cost Jacobians are inconsistent!";
309  }
310  catch (const std::exception& e)
311  {
312  ADD_FAILURE() << "Uncaught exception! " << e.what();
313  }
314 }
315 
316 TEST(ExoticaProblems, EndPoseProblem)
317 {
318  try
319  {
320  std::vector<Eigen::VectorXd> X(9);
321  std::vector<Eigen::MatrixXd> J(6); // Memory layout: {0,1 => Cost, 2,3 => Eq., 4,5 => Neq.}
322  for (int d = 0; d < 3; ++d)
323  {
324  CREATE_PROBLEM(EndPoseProblem, d);
325  Eigen::VectorXd x = problem->GetStartState();
326  TEST_COUT << "EndPoseProblem: Testing problem update";
327  problem->Update(x);
328  TEST_COUT << "EndPoseProblem::Update() - Test passed";
329  {
330  TEST_COUT << "EndPoseProblem: Testing cost";
331  X[d] = problem->cost.ydiff;
332  if (d > 0)
333  {
334  J[d - 1] = problem->cost.jacobian;
335  testJacobianEndPose(problem, problem->cost);
336  if (d > 1)
337  {
338  testHessianEndPose(problem, problem->cost);
339  }
340  }
341  }
342  problem->Update(x);
343  {
344  TEST_COUT << "EndPoseProblem: Testing equality";
345  X[d + 3] = problem->equality.ydiff;
346  if (d > 0)
347  {
348  J[d + 1] = problem->equality.jacobian;
349  testJacobianEndPose(problem, problem->equality);
350  if (d > 1)
351  {
352  testHessianEndPose(problem, problem->equality);
353  }
354  }
355  }
356  problem->Update(x);
357  {
358  TEST_COUT << "EndPoseProblem: Testing inequality";
359  X[d + 6] = problem->inequality.ydiff;
360  if (d > 0)
361  {
362  J[d + 3] = problem->inequality.jacobian;
363  testJacobianEndPose(problem, problem->inequality);
364  if (d > 1)
365  {
366  testHessianEndPose(problem, problem->inequality);
367  }
368  }
369  }
370  }
371  if (!(X[0] == X[1] && X[1] == X[2]))
372  ADD_FAILURE() << "EndPoseProblem: Cost value computation is inconsistent!";
373  if (!(J[0] == J[1]))
374  ADD_FAILURE() << "EndPoseProblem: Cost Jacobians are inconsistent!";
375  if (!(X[3] == X[4] && X[4] == X[5]))
376  ADD_FAILURE() << "EndPoseProblem: Equality value computation is inconsistent!";
377  if (!(J[2] == J[3]))
378  ADD_FAILURE() << "EndPoseProblem: Equality Jacobians are inconsistent!";
379  if (!(X[6] == X[7] && X[7] == X[8]))
380  ADD_FAILURE() << "EndPoseProblem: Inequality value computation is inconsistent!";
381  if (!(J[4] == J[5]))
382  ADD_FAILURE() << "EndPoseProblem: Inequality Jacobians are inconsistent!";
383  }
384  catch (const std::exception& e)
385  {
386  ADD_FAILURE() << "Uncaught exception! " << e.what();
387  }
388 }
389 
390 TEST(ExoticaProblems, UnconstrainedTimeIndexedProblem)
391 {
392  try
393  {
394  int T;
395  {
396  CREATE_PROBLEM(UnconstrainedTimeIndexedProblem, 0);
397  T = problem->GetT();
398  }
399  for (int t = 0; t < T; ++t)
400  {
401  std::vector<Eigen::VectorXd> X(3);
402  std::vector<Eigen::MatrixXd> J(3);
403  for (int d = 0; d < 3; ++d)
404  {
405  CREATE_PROBLEM(UnconstrainedTimeIndexedProblem, d);
406  Eigen::VectorXd x = problem->GetStartState();
407  TEST_COUT << "Testing problem update";
408  problem->Update(x, t);
409  TEST_COUT << "Test passed";
410  X[d] = problem->cost.ydiff[t];
411  TEST_COUT << "Testing cost";
412  if (d > 0)
413  {
414  J[d] = problem->cost.jacobian[t];
415  testJacobianTimeIndexed(problem, problem->cost, t);
416  if (d > 1)
417  {
418  testHessianTimeIndexed(problem, problem->cost, t);
419  }
420  }
421  }
422  if (!(X[0] == X[1] && X[1] == X[2]))
423  ADD_FAILURE() << "Cost FK is inconsistent!";
424  if (!(J[1] == J[2]))
425  ADD_FAILURE() << "Cost Jacobians are inconsistent!";
426  }
427  }
428  catch (const std::exception& e)
429  {
430  ADD_FAILURE() << "Uncaught exception! " << e.what();
431  }
432 }
433 
434 TEST(ExoticaProblems, BoundedTimeIndexedProblem)
435 {
436  try
437  {
438  int T;
439  {
440  CREATE_PROBLEM(BoundedTimeIndexedProblem, 0);
441  T = problem->GetT();
442  }
443  for (int t = 0; t < T; ++t)
444  {
445  std::vector<Eigen::VectorXd> X(3);
446  std::vector<Eigen::MatrixXd> J(3);
447  for (int d = 0; d < 3; ++d)
448  {
449  CREATE_PROBLEM(BoundedTimeIndexedProblem, d);
450  Eigen::VectorXd x = problem->GetStartState();
451  TEST_COUT << "BoundedTimeIndexedProblem: Testing problem update";
452  problem->Update(x, t);
453  TEST_COUT << "BoundedTimeIndexedProblem::Update(x): Test passed";
454  X[d] = problem->cost.ydiff[t];
455  TEST_COUT << "BoundedTimeIndexedProblem: Testing cost";
456  if (d > 0)
457  {
458  J[d] = problem->cost.jacobian[t];
459  testJacobianTimeIndexed(problem, problem->cost, t);
460  if (d > 1)
461  {
462  testHessianTimeIndexed(problem, problem->cost, t);
463  }
464  }
465  }
466  if (!(X[0].isApprox(X[1]) && X[1].isApprox(X[2])))
467  ADD_FAILURE() << "BoundedTimeIndexedProblem: cost value computation is inconsistent!";
468  if (!(J[1].isApprox(J[2])))
469  ADD_FAILURE() << "BoundedTimeIndexedProblem: cost Jacobians are inconsistent!";
470  }
471  }
472  catch (const std::exception& e)
473  {
474  ADD_FAILURE() << "Uncaught exception! " << e.what();
475  }
476 }
477 
478 TEST(ExoticaProblems, TimeIndexedProblem)
479 {
480  try
481  {
482  int T;
483  {
484  CREATE_PROBLEM(TimeIndexedProblem, 0);
485  T = problem->GetT();
486  }
487  for (int t = 0; t < T; ++t)
488  {
489  std::vector<Eigen::VectorXd> X(9);
490  std::vector<Eigen::MatrixXd> J(9);
491  for (int d = 0; d < 3; ++d)
492  {
493  CREATE_PROBLEM(TimeIndexedProblem, d);
494  Eigen::VectorXd x = problem->GetStartState();
495  TEST_COUT << "Testing problem update";
496  problem->Update(x, t);
497  TEST_COUT << "Test passed";
498  {
499  TEST_COUT << "Testing cost";
500  X[d] = problem->cost.ydiff[t];
501  if (d > 0)
502  {
503  J[d] = problem->cost.jacobian[t];
504  testJacobianTimeIndexed(problem, problem->cost, t);
505  if (d > 1)
506  {
507  testHessianTimeIndexed(problem, problem->cost, t);
508  }
509  }
510  }
511  problem->Update(x, t);
512  {
513  TEST_COUT << "Testing equality";
514  X[d + 3] = problem->equality.ydiff[t];
515  if (d > 0)
516  {
517  J[d + 3] = problem->equality.jacobian[t];
518  testJacobianTimeIndexed(problem, problem->equality, t);
519  if (d > 1)
520  {
521  testHessianTimeIndexed(problem, problem->equality, t);
522  }
523  }
524  }
525  problem->Update(x, t);
526  {
527  TEST_COUT << "Testing inequality";
528  X[d + 6] = problem->inequality.ydiff[t];
529  if (d > 0)
530  {
531  J[d + 6] = problem->inequality.jacobian[t];
532  testJacobianTimeIndexed(problem, problem->inequality, t);
533  if (d > 1)
534  {
535  testHessianTimeIndexed(problem, problem->inequality, t);
536  }
537  }
538  }
539  }
540  if (!(X[0] == X[1] && X[1] == X[2]))
541  ADD_FAILURE() << "Cost FK is inconsistent!";
542  if (!(J[1] == J[2]))
543  ADD_FAILURE() << "Cost Jacobians are inconsistent!";
544  if (!(X[3] == X[4] && X[4] == X[5]))
545  ADD_FAILURE() << "Equality FK is inconsistent!";
546  if (!(J[4] == J[5]))
547  ADD_FAILURE() << "Equality Jacobians are inconsistent!";
548  if (!(X[6] == X[7] && X[7] == X[8]))
549  ADD_FAILURE() << "Inequality FK is inconsistent!";
550  if (!(J[7] == J[8]))
551  ADD_FAILURE() << "Inequality Jacobians are inconsistent!";
552  }
553  }
554  catch (const std::exception& e)
555  {
556  ADD_FAILURE() << "Uncaught exception! " << e.what();
557  }
558 }
559 
560 TEST(ExoticaProblems, SamplingProblem)
561 {
562  try
563  {
564  CREATE_PROBLEM(SamplingProblem, 0);
565  Eigen::VectorXd x = problem->GetStartState();
566  TEST_COUT << "Testing problem update";
567  problem->Update(x);
568  TEST_COUT << "Testing valid state";
569  if (!problem->IsStateValid(x)) ADD_FAILURE() << "Start state is invalid!";
570  }
571  catch (const std::exception& e)
572  {
573  ADD_FAILURE() << "Uncaught exception! " << e.what();
574  }
575 }
576 
577 TEST(ExoticaProblems, TimeIndexedSamplingProblem)
578 {
579  try
580  {
581  CREATE_PROBLEM(TimeIndexedSamplingProblem, 0);
582  Eigen::VectorXd x = problem->GetStartState();
583  TEST_COUT << "Testing problem update";
584  problem->Update(x, 0.0);
585  TEST_COUT << "Testing valid state";
586  if (!problem->IsValid(x, 0.0)) ADD_FAILURE() << "Start state is invalid!";
587  }
588  catch (const std::exception& e)
589  {
590  ADD_FAILURE() << "Uncaught exception! " << e.what();
591  }
592 }
593 
594 int main(int argc, char** argv)
595 {
596  testing::InitGoogleTest(&argc, argv);
597  int ret = RUN_ALL_TESTS();
598  Setup::Destroy();
599  return ret;
600 }
T
int T
init
void init(bool compute_local_aabb=true)
exotica::UnconstrainedEndPoseProblem
main
int main(int argc, char **argv)
Definition: test_problems.cpp:594
exotica::TimeIndexedTask::hessian
std::vector< Hessian > hessian
exotica::EndPoseTask::hessian
Hessian hessian
exotica_core.h
operator-
template TMatrix3< double > operator-(const Matrix3< double > &m1, const TMatrix3< double > &m2)
exotica::TimeIndexedTask::Phi
std::vector< TaskSpaceVector > Phi
B
B
exotica::BoundedTimeIndexedProblem
J
J
i
int i
exotica::EndPoseTask::jacobian
Eigen::MatrixXd jacobian
exotica::Setup::CreateProblem
static std::shared_ptr< exotica::PlanningProblem > CreateProblem(const Initializer &init)
test.test_dynamic_time_indexed_shooting_problem.problem
problem
Definition: test_dynamic_time_indexed_shooting_problem.py:201
CREATE_PROBLEM
#define CREATE_PROBLEM(X, I)
Definition: test_problems.cpp:38
exotica::SamplingProblem
exotica::Setup::Destroy
static void Destroy()
exotica
Hessian
Eigen::Array< Eigen::MatrixXd, Eigen::Dynamic, 1 > Hessian
exotica::EndPoseTask::Phi
TaskSpaceVector Phi
exotica::TimeIndexedProblem
testHessianEndPose
void testHessianEndPose(std::shared_ptr< T > problem, EndPoseTask &task, double eps=1e-4, double h=1e-5)
Definition: test_problems.cpp:106
A
TEST
TEST(ExoticaProblems, UnconstrainedEndPoseProblem)
Definition: test_problems.cpp:244
exotica::TaskSpaceVector
testJacobianEndPose
void testJacobianEndPose(std::shared_ptr< T > problem, EndPoseTask &task, double eps=1e-4, double h=1e-5)
Definition: test_problems.cpp:73
exotica::eps
constexpr double eps
testHessianTimeIndexed
void testHessianTimeIndexed(std::shared_ptr< T > problem, TimeIndexedTask &task, int t, double eps=1e-4, double h=1e-5)
Definition: test_problems.cpp:192
x
x
exotica::TimeIndexedTask
exotica::Initializer
name
name
exotica::TimeIndexedSamplingProblem
h
h
NUM_TRIALS
#define NUM_TRIALS
Definition: test_problems.cpp:39
CreateProblem
std::shared_ptr< T > CreateProblem(const std::string &name, int derivative)
Definition: test_problems.cpp:60
exotica::TimeIndexedTask::jacobian
std::vector< Eigen::MatrixXd > jacobian
exotica::UnconstrainedTimeIndexedProblem
exotica::EndPoseProblem
exotica::EndPoseTask
exotica::XMLLoader::Load
static Initializer Load(std::string file_name, bool parsePathAsXML=false)
test_helpers.h
exotica::BoundedEndPoseProblem
exotica::Property
t
Transform3f t
testJacobianTimeIndexed
void testJacobianTimeIndexed(std::shared_ptr< T > problem, TimeIndexedTask &task, int t, double eps=1e-4, double h=1e-5)
Definition: test_problems.cpp:159
x0
x0
init
d
FCL_REAL d
X
TEST_COUT
#define TEST_COUT
isApprox
bool isApprox(const Box &s1, const Box &s2, const FCL_REAL tol)

exotica_examples
Author(s):
autogenerated on Fri Aug 2 2024 08:45:27