test_maps.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018-2020, University of Edinburgh, University of Oxford
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 <gtest/gtest.h>
31 #include <ros/ros.h>
32 
33 #include <exotica_core/exotica_core.h>
34 #include <exotica_core/tools/test_helpers.h>
35 
36 // TODO(#437): Activate once solution for pointer casting/dynamic loading is found.
37 // #include <exotica_core_task_maps/JointAccelerationBackwardDifference.h>
38 // #include <exotica_core_task_maps/JointJerkBackwardDifference.h>
39 // #include <exotica_core_task_maps/JointVelocityBackwardDifference.h>
40 
41 using namespace exotica;
42 
43 static const std::string urdf_string_ = "<robot name=\"test_robot\"><link name=\"base\"><visual><geometry><cylinder length=\"0.3\" radius=\"0.2\"/></geometry><origin xyz=\"0 0 0.15\"/></visual><collision><geometry><cylinder length=\"0.3\" radius=\"0.2\"/></geometry><origin xyz=\"0 0 0.15\"/></collision></link><link name=\"link1\"><inertial><mass value=\"0.2\"/><origin xyz=\"0 0 0.1\"/><inertia ixx=\"0.00381666666667\" ixy=\"0\" ixz=\"0\" iyy=\"0.0036\" iyz=\"0\" izz=\"0.00381666666667\"/></inertial><visual><geometry><cylinder length=\"0.15\" radius=\"0.05\"/></geometry><origin xyz=\"0 0 0.075\"/></visual><collision><geometry><cylinder length=\"0.15\" radius=\"0.05\"/></geometry><origin xyz=\"0 0 0.075\"/></collision></link><link name=\"link2\"><inertial><mass value=\"0.2\"/><origin xyz=\"0 0 0.1\"/><inertia ixx=\"0.00381666666667\" ixy=\"0\" ixz=\"0\" iyy=\"0.0036\" iyz=\"0\" izz=\"0.00381666666667\"/></inertial><visual><geometry><cylinder length=\"0.35\" radius=\"0.05\"/></geometry><origin xyz=\"0 0 0.175\"/></visual><collision><geometry><cylinder length=\"0.35\" radius=\"0.05\"/></geometry><origin xyz=\"0 0 0.175\"/></collision></link><link name=\"link3\"><inertial><mass value=\"0.2\"/><origin xyz=\"0 0 0.1\"/><inertia ixx=\"0.00381666666667\" ixy=\"0\" ixz=\"0\" iyy=\"0.0036\" iyz=\"0\" izz=\"0.00381666666667\"/></inertial><visual><geometry><cylinder length=\"0.45\" radius=\"0.05\"/></geometry><origin xyz=\"0 0 0.225\"/></visual><collision><geometry><cylinder length=\"0.45\" radius=\"0.05\"/></geometry><origin xyz=\"0 0 0.225\"/></collision></link><link name=\"endeff\"><inertial><mass value=\"0.2\"/><origin xyz=\"0 0 0.1\"/><inertia ixx=\"0.00381666666667\" ixy=\"0\" ixz=\"0\" iyy=\"0.0036\" iyz=\"0\" izz=\"0.00381666666667\"/></inertial><visual><geometry><cylinder length=\"0.05\" radius=\"0.1\"/></geometry><origin xyz=\"0 0 -0.025\"/></visual><collision><geometry><cylinder length=\"0.05\" radius=\"0.1\"/></geometry><origin xyz=\"0 0 -0.025\"/></collision></link><joint name=\"joint1\" type=\"revolute\"><parent link=\"base\"/><child link=\"link1\"/><origin xyz=\"0 0 0.3\" rpy=\"0 0 0\" /><axis xyz=\"0 0 1\" /><limit effort=\"200\" velocity=\"1.0\" lower=\"-0.4\" upper=\"0.4\"/><safety_controller k_position=\"30\" k_velocity=\"30\" soft_lower_limit=\"-0.4\" soft_upper_limit=\"0.4\"/></joint><joint name=\"joint2\" type=\"revolute\"><parent link=\"link1\"/><child link=\"link2\"/><origin xyz=\"0 0 0.15\" rpy=\"0 0 0\" /><axis xyz=\"0 1 0\" /><limit effort=\"200\" velocity=\"1.0\" lower=\"-0.4\" upper=\"0.4\"/><safety_controller k_position=\"30\" k_velocity=\"30\" soft_lower_limit=\"-0.4\" soft_upper_limit=\"0.4\"/></joint><joint name=\"joint3\" type=\"revolute\"><parent link=\"link2\"/><child link=\"link3\"/><origin xyz=\"0 0 0.35\" rpy=\"0 0 0\" /><axis xyz=\"0 1 0\" /><limit effort=\"200\" velocity=\"1.0\" lower=\"-0.4\" upper=\"0.4\"/><safety_controller k_position=\"30\" k_velocity=\"30\" soft_lower_limit=\"-0.4\" soft_upper_limit=\"0.4\"/></joint><joint name=\"joint4\" type=\"fixed\"><parent link=\"link3\"/><child link=\"endeff\"/><origin xyz=\"0 0 0.45\" rpy=\"0 0 0\" /></joint></robot>";
44 static const std::string srdf_string_ = "<robot name=\"test_robot\"><group name=\"arm\"><chain base_link=\"base\" tip_link=\"endeff\" /></group><virtual_joint name=\"world_joint\" type=\"fixed\" parent_frame=\"world_frame\" child_link=\"base\" /><group_state name=\"zero\" group=\"arm\"><joint name=\"joint1\" value=\"0\" /><joint name=\"joint2\" value=\"0.3\" /><joint name=\"joint3\" value=\"0.55\" /></group_state><disable_collisions link1=\"base\" link2=\"link1\" reason=\"Adjacent\" /><disable_collisions link1=\"endeff\" link2=\"link3\" reason=\"Adjacent\" /><disable_collisions link1=\"link1\" link2=\"link2\" reason=\"Adjacent\" /><disable_collisions link1=\"link2\" link2=\"link3\" reason=\"Adjacent\" /></robot>";
45 
46 constexpr bool print_debug_information_ = false;
47 constexpr int num_trials_ = 100;
48 
49 bool test_random(UnconstrainedEndPoseProblemPtr problem)
50 {
51  Eigen::VectorXd x(3);
52  TEST_COUT << "Testing random configurations:";
53  for (int i = 0; i < num_trials_; ++i)
54  {
55  x = problem->GetScene()->GetKinematicTree().GetRandomControlledState();
56  problem->Update(x);
57  if (print_debug_information_)
58  {
59  TEST_COUT << "x = " << x.transpose();
60  TEST_COUT << "y = " << problem->Phi.data.transpose();
61  TEST_COUT << "jacobian = \n"
62  << problem->jacobian;
63  }
64  }
65  return true;
66 }
67 
68 bool test_random(UnconstrainedTimeIndexedProblemPtr problem)
69 {
70  TEST_COUT << "Testing random configurations:";
71  for (int i = 0; i < num_trials_; ++i)
72  {
73  for (int t = 0; t < problem->GetT(); ++t)
74  {
75  problem->Update(problem->GetScene()->GetKinematicTree().GetRandomControlledState(), t);
76  }
77  }
78  return true;
79 }
80 
81 bool test_values(Eigen::MatrixXdRefConst Xref, Eigen::MatrixXdRefConst Yref, Eigen::MatrixXdRefConst Jref, UnconstrainedEndPoseProblemPtr problem, const double eps = 1e-5)
82 {
83  TEST_COUT << "Testing set points:";
84  int M = Yref.cols();
85  int L = Xref.rows();
86  for (int i = 0; i < L; ++i)
87  {
88  Eigen::VectorXd x = Xref.row(i);
89  TaskSpaceVector y = problem->cost.y;
90  y.data = Yref.row(i);
91  Eigen::MatrixXd jacobian = Jref.middleRows(i * M, M);
92  problem->Update(x);
93  double errY = (y - problem->Phi).norm();
94  double errJ = (jacobian - problem->jacobian).norm();
95  if (errY > eps)
96  {
97  TEST_COUT << "y: " << problem->Phi.data.transpose();
98  TEST_COUT << "y*: " << y.data.transpose();
99  ADD_FAILURE() << "Task space error out of bounds: " << errY;
100  }
101  if (errJ > eps)
102  {
103  TEST_COUT << "x: " << x.transpose();
104  TEST_COUT << "J*:\n"
105  << jacobian;
106  TEST_COUT << "J:\n"
107  << problem->jacobian;
108  ADD_FAILURE() << "Jacobian error out of bounds: " << errJ;
109  }
110  }
111 
112  return true;
113 }
114 
115 bool test_jacobian(UnconstrainedEndPoseProblemPtr problem, const double eps = 1e-5)
116 {
117  constexpr double h = 1e-5; // NB: Not, this differs from the h for the time-indexed Jacobian
118 
119  TEST_COUT << "Testing Jacobian with h=" << h << ", eps=" << eps;
120  for (int j = 0; j < num_trials_; ++j)
121  {
122  Eigen::VectorXd x0(problem->N);
123  x0 = problem->GetScene()->GetKinematicTree().GetRandomControlledState();
124  problem->Update(x0);
125  Eigen::VectorXd x(x0);
126  const Eigen::MatrixXd J0(problem->jacobian);
127  Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(J0.rows(), J0.cols());
128  for (int i = 0; i < problem->N; ++i)
129  {
130  x = x0;
131  x(i) += h;
132  problem->Update(x);
133  TaskSpaceVector x_plus(problem->Phi);
134  x = x0;
135  x(i) -= h;
136  problem->Update(x);
137  TaskSpaceVector x_minus(problem->Phi);
138  jacobian.col(i) = (x_plus - x_minus) / (2.0 * h);
139  }
140  double errJ = (jacobian - J0).lpNorm<Eigen::Infinity>();
141  if (errJ > eps)
142  {
143  TEST_COUT << "x: " << x0.transpose();
144  TEST_COUT << "J*:\n"
145  << jacobian;
146  TEST_COUT << "J:\n"
147  << J0;
148  TEST_COUT << "(J*-J):\n"
149  << (jacobian - J0);
150  ADD_FAILURE() << "Jacobian error out of bounds: " << errJ;
151  }
152  }
153  return true;
154 }
155 
156 bool test_hessian(UnconstrainedEndPoseProblemPtr problem, const double eps = 1e-5)
157 {
158  constexpr double h = 1e-5;
159 
160  TEST_COUT << "Testing Hessian with h=" << h << ", eps=" << eps;
161  for (int l = 0; l < num_trials_; ++l)
162  {
163  Eigen::VectorXd x0 = problem->GetScene()->GetKinematicTree().GetRandomControlledState();
164  problem->Update(x0);
165  const Hessian H0(problem->hessian);
166  Hessian hessian = Hessian::Constant(problem->length_jacobian, Eigen::MatrixXd::Zero(problem->N, problem->N));
167  Eigen::VectorXd x;
168  for (int j = 0; j < problem->N; ++j)
169  {
170  x = x0;
171  x(j) += h;
172  problem->Update(x);
173  const Eigen::MatrixXd J1 = problem->jacobian;
174  x = x0;
175  x(j) -= h;
176  problem->Update(x);
177  const Eigen::MatrixXd J2 = problem->jacobian;
178  for (int i = 0; i < problem->N; ++i)
179  {
180  for (int k = 0; k < problem->length_jacobian; ++k)
181  {
182  hessian(k)(i, j) = (J1(k, i) - J2(k, i)) / (2.0 * h);
183  }
184  }
185  }
186  double errH = 0;
187  for (int i = 0; i < hessian.rows(); ++i) errH += (hessian(i) - H0(i)).norm();
188  if (errH > eps)
189  {
190  TEST_COUT << "x: " << x0.transpose();
191  TEST_COUT << "H*:\n";
192  for (int i = 0; i < problem->length_jacobian; ++i) TEST_COUT << hessian(i) << "\n\n";
193  TEST_COUT << "\n\n...";
194  TEST_COUT << "H:\n";
195  for (int i = 0; i < problem->length_jacobian; ++i) TEST_COUT << H0(i) << "\n\n";
196  TEST_COUT << "\n\n...";
197  ADD_FAILURE() << "Hessian error out of bounds: " << errH;
198  }
199  }
200  return true;
201 }
202 
203 template <class T>
204 bool test_jacobian_time_indexed(std::shared_ptr<T> problem, TimeIndexedTask& task, int t, const double eps = 1e-5)
205 {
206  constexpr double h = 1e-6;
207 
208  TEST_COUT << "Testing Jacobian with h=" << h << ", eps=" << eps;
209  for (int tr = 0; tr < num_trials_; ++tr)
210  {
211  Eigen::VectorXd x0(problem->N);
212  x0 = problem->GetScene()->GetKinematicTree().GetRandomControlledState();
213  problem->Update(x0, t);
214  TaskSpaceVector y0 = task.Phi[t];
215  Eigen::MatrixXd J0 = task.jacobian[t];
216  Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(J0.rows(), J0.cols());
217  for (int i = 0; i < problem->N; ++i)
218  {
219  Eigen::VectorXd x = x0;
220  x(i) += h;
221  problem->Update(x, t);
222  jacobian.col(i) = (task.Phi[t] - y0) / h;
223  }
224  double errJ = (jacobian - J0).norm();
225  if (errJ > eps)
226  {
227  TEST_COUT << "x: " << x0.transpose();
228  TEST_COUT << "Phi: " << task.Phi[t].data.transpose();
229  TEST_COUT << "J*:\n"
230  << jacobian;
231  TEST_COUT << "J:\n"
232  << J0;
233  ADD_FAILURE() << "Jacobian error out of bounds: " << errJ;
234  }
235  }
236  return true;
237 }
238 
239 UnconstrainedEndPoseProblemPtr setup_problem(Initializer& map, const std::string& collision_scene = "", const std::vector<Initializer>& links = std::vector<Initializer>())
240 {
241  Initializer scene;
242  if (!collision_scene.empty())
243  scene = Initializer("Scene", {{"Name", std::string("MyScene")},
244  {"JointGroup", std::string("arm")},
245  {"Links", links},
246  {"AlwaysUpdateCollisionScene", std::string("1")},
247  {"CollisionScene", std::vector<Initializer>({Initializer(collision_scene, {{"Name", std::string("MyCollisionScene")}})})}});
248  else
249  scene = Initializer("Scene", {{"Name", std::string("MyScene")}, {"JointGroup", std::string("arm")}, {"Links", links}});
250  Initializer cost("exotica/Task", {{"Task", std::string("MyTask")}});
251  Eigen::VectorXd W = Eigen::Vector3d(3, 2, 1);
252  Initializer problem("exotica/UnconstrainedEndPoseProblem", {
253  {"Name", std::string("MyProblem")},
254  {"DerivativeOrder", std::string("2")},
255  {"PlanningScene", scene},
256  {"Maps", std::vector<Initializer>({map})},
257  {"Cost", std::vector<Initializer>({cost})},
258  {"W", W},
259  });
260  Server::Instance()->GetModel("robot_description", urdf_string_, srdf_string_);
261 
262  UnconstrainedEndPoseProblemPtr problem_ptr = std::static_pointer_cast<UnconstrainedEndPoseProblem>(Setup::CreateProblem(problem));
263 
264  // Create and test a problem with multiple cost terms
265  problem = Initializer("exotica/UnconstrainedEndPoseProblem", {
266  {"Name", std::string("MyProblem")},
267  {"PlanningScene", scene},
268  {"Maps", std::vector<Initializer>({map})},
269  {"Cost", std::vector<Initializer>({cost, cost, cost})},
270  {"W", W},
271  });
272 
273  test_random(std::static_pointer_cast<UnconstrainedEndPoseProblem>(Setup::CreateProblem(problem)));
274 
275  return problem_ptr;
276 }
277 
278 UnconstrainedTimeIndexedProblemPtr setup_time_indexed_problem(Initializer& map)
279 {
280  Initializer scene("Scene", {{"Name", std::string("MyScene")}, {"JointGroup", std::string("arm")}});
281  Initializer cost("exotica/Task", {{"Task", std::string("MyTask")}});
282  Eigen::VectorXd W = Eigen::Vector3d(3, 2, 1);
283 
284  Initializer problem("exotica/UnconstrainedTimeIndexedProblem", {{"Name", std::string("MyProblem")},
285  {"PlanningScene", scene},
286  {"Maps", std::vector<Initializer>({map})},
287  {"Cost", std::vector<Initializer>({cost})},
288  {"W", W},
289  {"T", std::string("10")},
290  {"tau", std::string("0.05")}});
291  Server::Instance()->GetModel("robot_description", urdf_string_, srdf_string_);
292 
293  UnconstrainedTimeIndexedProblemPtr problem_ptr = std::static_pointer_cast<UnconstrainedTimeIndexedProblem>(Setup::CreateProblem(problem));
294 
295  // Create and test a problem with multiple cost terms
296  problem = Initializer("exotica/UnconstrainedTimeIndexedProblem", {{"Name", std::string("MyProblem")},
297  {"PlanningScene", scene},
298  {"Maps", std::vector<Initializer>({map})},
299  {"Cost", std::vector<Initializer>({cost, cost, cost})},
300  {"W", W},
301  {"T", std::string("10")},
302  {"tau", std::string("0.05")}});
303 
304  test_random(std::static_pointer_cast<UnconstrainedTimeIndexedProblem>(Setup::CreateProblem(problem)));
305 
306  return problem_ptr;
307 }
308 
309 TEST(ExoticaTaskMaps, testEffPositionXY)
310 {
311  try
312  {
313  TEST_COUT << "End-effector position XY test";
314  Initializer map("exotica/EffPositionXY", {{"Name", std::string("MyTask")},
315  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("endeff")}})})}});
316  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
317  EXPECT_TRUE(test_random(problem));
318  EXPECT_TRUE(test_jacobian(problem));
319  EXPECT_TRUE(test_hessian(problem));
320  }
321  catch (const std::exception& e)
322  {
323  ADD_FAILURE() << "Uncaught exception! " << e.what();
324  }
325 }
326 
327 TEST(ExoticaTaskMaps, testEffPosition)
328 {
329  try
330  {
331  TEST_COUT << "End-effector position test";
332  Initializer map("exotica/EffPosition", {{"Name", std::string("MyTask")},
333  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("endeff")}})})}});
334  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
335  EXPECT_TRUE(test_random(problem));
336 
337  int N = problem->N;
338  int M = problem->length_Phi;
339  int L = 5;
340  Eigen::MatrixXd X(L, N);
341  Eigen::MatrixXd Y(L, M);
342  Eigen::MatrixXd jacobian(L * M, N);
343 
344  X << 0.680375, -0.211234, 0.566198, 0.59688, 0.823295, -0.604897, -0.329554, 0.536459, -0.444451, 0.10794, -0.0452059, 0.257742, -0.270431, 0.0268018, 0.904459;
345  Y << 0.0645323, 0.0522249, 1.21417, 0.292945, 0.199075, 1.12724, 0.208378, -0.0712708, 1.19893, 0.0786457, 0.00852213, 1.23952, 0.356984, -0.0989639, 1.06844;
346  jacobian << -0.0522249, 0.594015, 0.327994, 0.0645323, 0.480726, 0.26544, 0, -0.0830172, -0.156401, -0.199075, 0.560144, 0.363351, 0.292945, 0.380655, 0.246921, 0, -0.354186, -0.0974994, 0.0712708, 0.708627, 0.423983, 0.208378, -0.24237, -0.145014, 0, -0.220229, -0.0413455, -0.00852213, 0.784922, 0.437315, 0.0786457, 0.085055, 0.0473879, 0, -0.0791061, -0.0949228, 0.0989639, 0.595968, 0.258809, 0.356984, -0.165215, -0.0717477, 0, -0.370448, -0.361068;
347  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
348 
349  EXPECT_TRUE(test_jacobian(problem));
350  EXPECT_TRUE(test_hessian(problem));
351  }
352  catch (const std::exception& e)
353  {
354  ADD_FAILURE() << "Uncaught exception! " << e.what();
355  }
356 }
357 
358 TEST(ExoticaTaskMaps, testEffOrientation)
359 {
360  try
361  {
362  TEST_COUT << "End-effector orientation test";
363  std::vector<std::string> types = {"Quaternion", "ZYX", "ZYZ", "AngleAxis", "Matrix", "RPY"};
364 
365  for (std::size_t i = 0; i < types.size(); ++i)
366  {
367  std::string type = types[i];
368  TEST_COUT << "Rotation type " << type;
369  Initializer map("exotica/EffOrientation", {{"Name", std::string("MyTask")},
370  {"Type", type},
371  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("endeff")}})})}});
372  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
373 
374  EXPECT_TRUE(test_random(problem));
375 
376  EXPECT_TRUE(test_jacobian(problem));
377  EXPECT_TRUE(test_hessian(problem));
378  }
379  }
380  catch (const std::exception& e)
381  {
382  ADD_FAILURE() << "Uncaught exception! " << e.what();
383  }
384 }
385 
386 TEST(ExoticaTaskMaps, testEffAxisAlignment)
387 {
388  try
389  {
390  TEST_COUT << "End-effector axis alignment test";
391 
392  Initializer map("exotica/EffAxisAlignment", {{"Name", std::string("MyTask")},
393  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("endeff")}, {"Axis", std::string("1 0 0")}, {"Direction", std::string("0 0 1")}})})}});
394  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
395  EXPECT_TRUE(test_random(problem));
396 
397  EXPECT_TRUE(test_jacobian(problem));
398  }
399  catch (const std::exception& e)
400  {
401  ADD_FAILURE() << "Uncaught exception! " << e.what();
402  }
403 }
404 
405 TEST(ExoticaTaskMaps, testEffFrame)
406 {
407  try
408  {
409  TEST_COUT << "End-effector frame test";
410  std::vector<std::string> types = {"Quaternion", "ZYX", "ZYZ", "AngleAxis", "Matrix", "RPY"};
411 
412  for (std::size_t i = 0; i < types.size(); ++i)
413  {
414  const std::string& type = types[i];
415  TEST_COUT << "Rotation type " << type;
416  Initializer map("exotica/EffFrame", {{"Name", std::string("MyTask")},
417  {"Type", type},
418  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("endeff")}})})}});
419  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
420  EXPECT_TRUE(test_random(problem));
421 
422  EXPECT_TRUE(test_jacobian(problem));
423  EXPECT_TRUE(test_hessian(problem));
424  }
425 
426  // Multi-end-effector test case
427  for (std::size_t i = 0; i < types.size(); ++i)
428  {
429  const std::string& type = types[i];
430  TEST_COUT << "[Multi end-effector] Rotation type " << type;
431  Initializer map("exotica/EffFrame", {{"Name", std::string("MyTask")},
432  {"Type", type},
433  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("endeff")}}),
434  Initializer("Frame", {{"Link", std::string("link3")}})})}});
435  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
436  EXPECT_TRUE(test_random(problem));
437 
438  EXPECT_TRUE(test_jacobian(problem));
439  EXPECT_TRUE(test_hessian(problem));
440  }
441  }
442  catch (const std::exception& e)
443  {
444  ADD_FAILURE() << "Uncaught exception! " << e.what();
445  }
446 }
447 
448 TEST(ExoticaTaskMaps, testEffVelocity)
449 {
450  try
451  {
452  TEST_COUT << "End-effector velocity test";
453 
454  Initializer map("exotica/EffVelocity", {{"Name", std::string("MyTask")},
455  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("endeff")}})})}});
456  UnconstrainedTimeIndexedProblemPtr problem = setup_time_indexed_problem(map);
457  EXPECT_TRUE(test_random(problem));
458 
459  for (int t = 0; t < problem->GetT(); ++t)
460  {
461  problem->Update(problem->GetScene()->GetKinematicTree().GetRandomControlledState(), t);
462  }
463 
464  for (int t = 0; t < problem->GetT(); ++t)
465  {
466  // TODO: Can we get those tolerances to be tighter?
467  EXPECT_TRUE(test_jacobian_time_indexed(problem, problem->cost, t, 1e-3));
468  }
469  }
470  catch (const std::exception& e)
471  {
472  ADD_FAILURE() << "Uncaught exception! " << e.what();
473  }
474 }
475 
476 TEST(ExoticaTaskMaps, testDistance)
477 {
478  try
479  {
480  TEST_COUT << "Distance test";
481  Initializer map("exotica/Distance", {{"Name", std::string("MyTask")},
482  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("endeff")}})})}});
483  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
484  EXPECT_TRUE(test_random(problem));
485 
486  int N = problem->N;
487  int M = problem->length_Phi;
488  int L = 5;
489  Eigen::MatrixXd X(L, N);
490  Eigen::MatrixXd Y(L, M);
491  Eigen::MatrixXd jacobian(L * M, N);
492 
493  X << 0.83239, 0.271423, 0.434594, -0.716795, 0.213938, -0.967399, -0.514226, -0.725537, 0.608354, -0.686642, -0.198111, -0.740419, -0.782382, 0.997849, -0.563486;
494  Y << 1.19368, 1.14431, 1.19326, 1.14377, 1.1541;
495  jacobian << 0, -0.145441, -0.16562,
496  0, 0.0918504, 0.234405,
497  0, 0.107422, -0.0555943,
498  0, 0.169924, 0.235715,
499  0, -0.188516, -0.000946239;
500  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
501 
502  EXPECT_TRUE(test_jacobian(problem));
503  EXPECT_TRUE(test_hessian(problem, 1e-2));
504  }
505  catch (const std::exception& e)
506  {
507  ADD_FAILURE() << "Uncaught exception! " << e.what();
508  }
509 }
510 
511 TEST(ExoticaTaskMaps, testDistanceToLine2D)
512 {
513  try
514  {
515  // Fixed frames build line
516  {
517  TEST_COUT << "DistanceToLine2D test: Fixed frames";
518  Initializer map("exotica/DistanceToLine2D", {{"Name", std::string("MyTask")},
519  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("base")}, {"BaseOffset", std::string("-0.5 -0.5 0")}}),
520  Initializer("Frame", {{"Link", std::string("base")}, {"BaseOffset", std::string("0.5 0.5 0")}}),
521  Initializer("Frame", {{"Link", std::string("endeff")}})})}});
522  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
523  EXPECT_TRUE(test_random(problem));
524  EXPECT_TRUE(test_jacobian(problem));
525  }
526 
527  // Robot (moving) frames build line
528  {
529  TEST_COUT << "DistanceToLine2D test: Moving links";
530  Initializer map("exotica/DistanceToLine2D", {{"Name", std::string("MyTask")},
531  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("link1")}}),
532  Initializer("Frame", {{"Link", std::string("link3")}}),
533  Initializer("Frame", {{"Link", std::string("endeff")}})})}});
534  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
535  EXPECT_TRUE(test_random(problem));
536  EXPECT_TRUE(test_jacobian(problem));
537  }
538  }
539  catch (const std::exception& e)
540  {
541  ADD_FAILURE() << "Uncaught exception! " << e.what();
542  }
543 }
544 
545 TEST(ExoticaTaskMaps, testJointLimit)
546 {
547  try
548  {
549  TEST_COUT << "Joint limit test";
550  Initializer map("exotica/JointLimit", {{"Name", std::string("MyTask")},
551  {"SafePercentage", 0.0}});
552  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
553  EXPECT_TRUE(test_random(problem));
554  EXPECT_TRUE(test_jacobian(problem));
555  EXPECT_TRUE(test_hessian(problem));
556 
557  // TODO: Test dynamic case (!)
558  }
559  catch (const std::exception& e)
560  {
561  ADD_FAILURE() << "Uncaught exception! " << e.what();
562  }
563 }
564 
565 TEST(ExoticaTaskMaps, testJointTorqueMinimizationProxy)
566 {
567  try
568  {
569  TEST_COUT << "Joint torque minimization proxy test";
570 
571  Initializer map("exotica/JointTorqueMinimizationProxy", {{"Name", std::string("MyTask")},
572  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("endeff")}})})}});
573  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
574  EXPECT_TRUE(test_random(problem));
575 
576  EXPECT_TRUE(test_jacobian(problem, 1.e-4));
577  }
578  catch (const std::exception& e)
579  {
580  ADD_FAILURE() << "Uncaught exception! " << e.what();
581  }
582 }
583 
584 /*TEST(ExoticaTaskMaps, testJointVelocityLimit)
585 {
586  try
587  {
588  TEST_COUT << "JointVelocityLimit test";
589 
590  std::vector<Initializer> maps;
591  maps.reserve(3);
592 
593  // Test default
594  {
595  Eigen::VectorXd qd_max = Eigen::VectorXd::Ones(1);
596  Initializer map("exotica/JointVelocityLimit", {{"Name", std::string("MyTask")},
597  {"dt", 0.1},
598  {"MaximumJointVelocity", qd_max},
599  {"SafePercentage", 0.0}});
600  maps.emplace_back(map);
601  }
602 
603  // Test safe percentage
604  {
605  Eigen::VectorXd qd_max = Eigen::VectorXd::Ones(1);
606  Initializer map("exotica/JointVelocityLimit", {{"Name", std::string("MyTask")},
607  {"dt", 0.1},
608  {"MaximumJointVelocity", qd_max},
609  {"SafePercentage", 0.1}});
610  maps.emplace_back(map);
611  }
612 
613  // Test different joint velocity initialisations (vector 1, vector N)
614  {
615  UnconstrainedTimeIndexedProblemPtr problem = setup_time_indexed_problem(maps[0]);
616  Eigen::VectorXd qd_max = Eigen::VectorXd::Ones(problem->N);
617  Initializer map("exotica/JointVelocityLimit", {{"Name", std::string("MyTask")},
618  {"dt", 0.1},
619  {"MaximumJointVelocity", qd_max},
620  {"SafePercentage", 0.0}});
621  maps.emplace_back(map);
622  }
623 
624  for (auto map : maps)
625  {
626  UnconstrainedTimeIndexedProblemPtr problem = setup_time_indexed_problem(map);
627  EXPECT_TRUE(test_random(problem));
628 
629  for (int t = 0; t < problem->GetT(); ++t)
630  {
631  problem->Update(problem->GetScene()->GetKinematicTree().GetRandomControlledState(), t);
632  }
633 
634  for (int t = 0; t < problem->GetT(); ++t)
635  {
636  EXPECT_TRUE(test_jacobian_time_indexed(problem, problem->cost, t, 1e-4));
637  }
638  }
639  }
640  catch (const std::exception& e)
641  {
642  ADD_FAILURE() << "JointVelocityLimit: Uncaught exception! " << e.what();
643  }
644 }*/
645 
646 TEST(ExoticaTaskMaps, testSphereCollision)
647 {
648  try
649  {
650  TEST_COUT << "Sphere collision test";
651  Initializer map("exotica/SphereCollision", {{"Name", std::string("MyTask")},
652  {"Precision", 1e-2},
653  {"ReferenceFrame", std::string("base")},
654  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("base")}, {"Radius", 0.3}, {"Group", std::string("base")}}),
655  Initializer("Frame", {{"Link", std::string("endeff")}, {"Radius", 0.3}, {"Group", std::string("eff")}})})}});
656  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
657  EXPECT_TRUE(test_random(problem));
658 
659  int N = problem->N;
660  int M = problem->length_Phi;
661  int L = 5;
662  Eigen::MatrixXd X(L, N);
663  Eigen::MatrixXd Y(L, M);
664  Eigen::MatrixXd jacobian(L * M, N);
665 
666  X << -0.590167, 1.2309, 1.67611, -1.72098, 1.79731, 0.103981, -1.65578, -1.23114, 0.652908, 1.56093, -0.604428, -1.74331, -1.91991, -0.169193, -1.74762;
667  Y << 1, 5.71023e-44, 1.83279e-110, 6.87352e-16, 7.26371e-45;
668  jacobian << 0, 0.431392, 0.449344,
669  0, 0.431735, 0.26014,
670  0, -0.234475, -0.0135658,
671  0, -0.349195, -0.447214,
672  0, -0.270171, -0.430172;
673  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
674 
675  // EXPECT_TRUE(test_jacobian(problem)); // Throws!
676  }
677  catch (const std::exception& e)
678  {
679  ADD_FAILURE() << "Uncaught exception! " << e.what();
680  }
681 }
682 
683 TEST(ExoticaTaskMaps, testJointPose)
684 {
685  try
686  {
687  TEST_COUT << "JointPose test";
688  Initializer map("exotica/JointPose", {{"Name", std::string("MyTask")}});
689  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
690  EXPECT_TRUE(test_random(problem));
691 
692  {
693  int N = problem->N;
694  int M = problem->length_Phi;
695  int L = 5;
696  Eigen::MatrixXd X(L, N);
697  Eigen::MatrixXd Y(L, M);
698  Eigen::MatrixXd jacobian(L * M, N);
699 
700  X << -0.52344, 0.941268, 0.804416, 0.70184, -0.466669, 0.0795207, -0.249586, 0.520497, 0.0250707, 0.335448, 0.0632129, -0.921439, -0.124725, 0.86367, 0.86162;
701  Y << -0.52344, 0.941268, 0.804416, 0.70184, -0.466669, 0.0795207, -0.249586, 0.520497, 0.0250707, 0.335448, 0.0632129, -0.921439, -0.124725, 0.86367, 0.86162;
702  jacobian << 1, 0, 0,
703  0, 1, 0,
704  0, 0, 1,
705  1, 0, 0,
706  0, 1, 0,
707  0, 0, 1,
708  1, 0, 0,
709  0, 1, 0,
710  0, 0, 1,
711  1, 0, 0,
712  0, 1, 0,
713  0, 0, 1,
714  1, 0, 0,
715  0, 1, 0,
716  0, 0, 1;
717  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
718  }
719  EXPECT_TRUE(test_jacobian(problem));
720  EXPECT_TRUE(test_hessian(problem));
721 
722  TEST_COUT << "JointPose test with reference";
723  map = Initializer("exotica/JointPose", {{"Name", std::string("MyTask")},
724  {"JointRef", std::string("0.5 0.5 0.5")}});
725  problem = setup_problem(map);
726  EXPECT_TRUE(test_random(problem));
727 
728  {
729  int N = problem->N;
730  int M = problem->length_Phi;
731  int L = 5;
732  Eigen::MatrixXd X(L, N);
733  Eigen::MatrixXd Y(L, M);
734  Eigen::MatrixXd jacobian(L * M, N);
735 
736  X << 0.441905, -0.431413, 0.477069, 0.279958, -0.291903, 0.375723, -0.668052, -0.119791, 0.76015, 0.658402, -0.339326, -0.542064, 0.786745, -0.29928, 0.37334;
737  Y << -0.0580953, -0.931413, -0.0229314, -0.220042, -0.791903, -0.124277, -1.16805, -0.619791, 0.26015, 0.158402, -0.839326, -1.04206, 0.286745, -0.79928, -0.12666;
738  jacobian << 1, 0, 0,
739  0, 1, 0,
740  0, 0, 1,
741  1, 0, 0,
742  0, 1, 0,
743  0, 0, 1,
744  1, 0, 0,
745  0, 1, 0,
746  0, 0, 1,
747  1, 0, 0,
748  0, 1, 0,
749  0, 0, 1,
750  1, 0, 0,
751  0, 1, 0,
752  0, 0, 1;
753  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
754  }
755  EXPECT_TRUE(test_jacobian(problem));
756  EXPECT_TRUE(test_hessian(problem));
757 
758  TEST_COUT << "JointPose test with subset of joints";
759  map = Initializer("exotica/JointPose", {{"Name", std::string("MyTask")},
760  {"JointMap", std::string("0")}});
761  problem = setup_problem(map);
762  EXPECT_TRUE(test_random(problem));
763 
764  {
765  int N = problem->N;
766  int M = problem->length_Phi;
767  int L = 5;
768  Eigen::MatrixXd X(L, N);
769  Eigen::MatrixXd Y(L, M);
770  Eigen::MatrixXd jacobian(L * M, N);
771 
772  X << 0.912937, 0.17728, 0.314608, 0.717353, -0.12088, 0.84794, -0.203127, 0.629534, 0.368437, 0.821944, -0.0350187, -0.56835, 0.900505, 0.840257, -0.70468;
773  Y << 0.912937, 0.717353, -0.203127, 0.821944, 0.900505;
774  jacobian << 1, 0, 0,
775  1, 0, 0,
776  1, 0, 0,
777  1, 0, 0,
778  1, 0, 0;
779  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
780  }
781  EXPECT_TRUE(test_jacobian(problem));
782  }
783  catch (const std::exception& e)
784  {
785  ADD_FAILURE() << "JointPose: Uncaught exception!" << e.what();
786  }
787 }
788 
789 TEST(ExoticaTaskMaps, testCoM)
790 {
791  try
792  {
793  TEST_COUT << "CoM test";
794  Initializer map("exotica/CenterOfMass", {{"Name", std::string("MyTask")},
795  {"EnableZ", true}});
796  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
797  EXPECT_TRUE(test_random(problem));
798 
799  {
800  int N = problem->N;
801  int M = problem->length_Phi;
802  int L = 5;
803  Eigen::MatrixXd X(L, N);
804  Eigen::MatrixXd Y(L, M);
805  Eigen::MatrixXd jacobian(L * M, N);
806 
807  X << -0.281809, 0.10497, 0.15886, -0.0948483, 0.374775, -0.80072, 0.061616, 0.514588, -0.39141, 0.984457, 0.153942, 0.755228, 0.495619, 0.25782, -0.929158;
808  Y << 0.081112, -0.0234831, 0.924368, 0.0080578, -0.000766568, 0.895465, 0.157569, 0.00972105, 0.897157, 0.117213, 0.176455, 0.846633, -0.0587457, -0.0317596, 0.877501;
809  jacobian << 0.0234831, 0.455657, 0.200919,
810  0.081112, -0.131919, -0.0581688,
811  0, -0.0844429, -0.0565023,
812  0.000766568, 0.443462, 0.19642,
813  0.0080578, -0.0421882, -0.0186862,
814  0, -0.00809418, 0.0895227,
815  -0.00972105, 0.446309, 0.214617,
816  0.157569, 0.0275346, 0.0132406,
817  0, -0.157868, -0.0266211,
818  -0.176455, 0.219463, 0.0736575,
819  0.117213, 0.330384, 0.110885,
820  0, -0.211838, -0.170949,
821  0.0317596, 0.376061, 0.149235,
822  -0.0587457, 0.203309, 0.0806805,
823  0, 0.0667812, 0.134774;
824  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
825  }
826  EXPECT_TRUE(test_jacobian(problem));
827 
828  TEST_COUT << "CoM test with a subset of links";
829  map = Initializer("exotica/CenterOfMass", {{"Name", std::string("MyTask")},
830  {"EnableZ", true},
831  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("link2")}}),
832  Initializer("Frame", {{"Link", std::string("endeff")}})})}});
833  problem = setup_problem(map);
834  EXPECT_TRUE(test_random(problem));
835 
836  {
837  int N = problem->N;
838  int M = problem->length_Phi;
839  int L = 5;
840  Eigen::MatrixXd X(L, N);
841  Eigen::MatrixXd Y(L, M);
842  Eigen::MatrixXd jacobian(L * M, N);
843 
844  X << 0.299414, -0.503912, 0.258959, -0.541726, 0.40124, -0.366266, -0.342446, -0.537144, -0.851678, 0.266144, -0.552687, 0.302264, 0.0213719, 0.942931, -0.439916;
845  Y << -0.167532, -0.0517162, 0.913823, 0.083533, -0.0502684, 0.931962, -0.3632, 0.129477, 0.693081, -0.17971, -0.048991, 0.907923, 0.314586, 0.00672432, 0.823106;
846  jacobian << 0.0517162, 0.443188, 0.254921,
847  -0.167532, 0.13681, 0.0786927,
848  0, 0.175333, 0.0666905,
849  0.0502684, 0.412954, 0.235481,
850  0.083533, -0.248507, -0.141708,
851  0, -0.0974919, -0.00961589,
852  -0.129477, 0.228967, 0.0468775,
853  -0.3632, -0.0816248, -0.0167114,
854  0, 0.385588, 0.270459,
855  0.048991, 0.441801, 0.257042,
856  -0.17971, 0.12044, 0.0700725,
857  0, 0.186268, 0.0681488,
858  -0.00672432, 0.373021, 0.240882,
859  0.314586, 0.00797337, 0.00514888,
860  0, -0.314658, -0.132569;
861  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
862  }
863  EXPECT_TRUE(test_jacobian(problem));
864 
865  TEST_COUT << "CoM test with projection on XY plane";
866  map = Initializer("exotica/CenterOfMass", {{"Name", std::string("MyTask")},
867  {"EnableZ", false}});
868  problem = setup_problem(map);
869  EXPECT_TRUE(test_random(problem));
870 
871  {
872  int N = problem->N;
873  int M = problem->length_Phi;
874  int L = 5;
875  Eigen::MatrixXd X(L, N);
876  Eigen::MatrixXd Y(L, M);
877  Eigen::MatrixXd jacobian(L * M, N);
878 
879  X << 0.350952, -0.0340994, -0.0361284, -0.390089, 0.424175, -0.634888, 0.243646, -0.918271, -0.172033, 0.391968, 0.347873, 0.27528, -0.305768, -0.630755, 0.218212;
880  Y << -0.0228141, -0.0083524, 0.0595937, -0.0245025, -0.392081, -0.0974653, 0.200868, 0.0830305, -0.232814, 0.0734919;
881  jacobian << 0.0083524, 0.453225, 0.202958,
882  -0.0228141, 0.165929, 0.0743046,
883  0.0245025, 0.420734, 0.195957,
884  0.0595937, -0.172988, -0.0805696,
885  0.0974653, 0.254325, 0.0971889,
886  -0.392081, 0.0632213, 0.0241597,
887  -0.0830305, 0.394279, 0.162599,
888  0.200868, 0.162978, 0.0672114,
889  -0.0734919, 0.394649, 0.189283,
890  -0.232814, -0.124578, -0.0597504;
891  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
892  }
893  EXPECT_TRUE(test_jacobian(problem));
894 
895  TEST_COUT << "CoM test with attached object";
896  map = Initializer("exotica/CenterOfMass", {{"Name", std::string("MyTask")},
897  {"EnableZ", true}});
898  problem = setup_problem(map);
899 
900  problem->GetScene()->AddObject("Payload", KDL::Frame(), "", shapes::ShapeConstPtr(nullptr), KDL::RigidBodyInertia(0.5));
901  problem->GetScene()->AttachObjectLocal("Payload", "endeff", KDL::Frame());
902  EXPECT_TRUE(test_random(problem));
903  {
904  int N = problem->N;
905  int M = problem->length_Phi;
906  int L = 5;
907  Eigen::MatrixXd X(L, N);
908  Eigen::MatrixXd Y(L, M);
909  Eigen::MatrixXd jacobian(L * M, N);
910 
911  X << 0.792099, -0.891848, -0.781543, 0.877611, 0.29783, 0.452939, 0.988809, 0.86931, 0.270667, -0.201327, -0.925895, 0.0373103, 0.0433417, 0.560965, -0.682102;
912  Y << -0.391939, -0.397228, 0.608195, 0.197788, 0.238097, 0.977105, 0.289066, 0.439301, 0.781316, -0.4839, 0.0987601, 0.836551, 0.122905, 0.00533025, 1.02823;
913  jacobian << 0.397228, 0.111109, -0.0232142,
914  -0.391939, 0.112608, -0.0235274,
915  0, 0.558038, 0.32103,
916  -0.238097, 0.336815, 0.150781,
917  0.197788, 0.405457, 0.181509,
918  0, -0.309533, -0.220165,
919  -0.439301, 0.182119, 0.0740843,
920  0.289066, 0.276772, 0.112588,
921  0, -0.525875, -0.293238,
922  -0.0987601, 0.378744, 0.199373,
923  -0.4839, -0.0772987, -0.0406905,
924  0, 0.493875, 0.250495,
925  -0.00533025, 0.577691, 0.320061,
926  0.122905, 0.0250538, 0.0138807,
927  0, -0.123021, 0.0389986;
928  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
929  }
930  EXPECT_TRUE(test_jacobian(problem));
931  }
932  catch (const std::exception& e)
933  {
934  ADD_FAILURE() << "Uncaught exception! " << e.what();
935  }
936 }
937 
938 TEST(ExoticaTaskMaps, testContinuousJointPose)
939 {
940  try
941  {
942  TEST_COUT << "ContinuousJointPose test";
943  Initializer map("exotica/ContinuousJointPose", {{"Name", std::string("MyTask")}, {}});
944  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
945  EXPECT_TRUE(test_random(problem));
946  EXPECT_TRUE(test_jacobian(problem));
947  EXPECT_TRUE(test_hessian(problem));
948  }
949  catch (const std::exception& e)
950  {
951  ADD_FAILURE() << "Uncaught exception! " << e.what();
952  }
953 }
954 
955 TEST(ExoticaTaskMaps, testIMesh)
956 {
957  try
958  {
959  TEST_COUT << "Interaction mesh test";
960  Initializer map("exotica/InteractionMesh", {{"Name", std::string("MyTask")},
961  {"ReferenceFrame", std::string("base")},
962  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("base")}}),
963  Initializer("Frame", {{"Link", std::string("link2")}}),
964  Initializer("Frame", {{"Link", std::string("endeff")}})})}});
965  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
966  EXPECT_TRUE(test_random(problem));
967 
968  int N = problem->N;
969  int M = problem->length_Phi;
970  int L = 5;
971  Eigen::MatrixXd X(L, N);
972  Eigen::MatrixXd Y(L, M);
973  Eigen::MatrixXd jacobian(L * M, N);
974  jacobian.setZero();
975  Y.setZero();
976  X.setZero();
977 
978  X << 0.278917, 0.51947, -0.813039, -0.730195, 0.0404201, -0.843536, -0.860187, -0.59069, -0.0771591, 0.639355, 0.146637, 0.511162, -0.896122, -0.684386, 0.999987;
979  Y << -0.0115151, -0.00329772, -0.652131, -0.01588, -0.00454774, 0.000489, 0.0418479, 0.0119845, 0.906934,
980  0.0647007, -0.0579245, -0.635726, 0.0879002, -0.0786944, 0.0262201, -0.230697, 0.206536, 0.836692,
981  0.0846586, -0.098373, -0.626482, 0.111269, -0.129294, 0.0559699, -0.308935, 0.358981, 0.824647,
982  -0.0715067, -0.0531681, -0.641823, -0.0962226, -0.0715454, 0.0264905, 0.261816, 0.194671, 0.879061,
983  0.014318, -0.0178999, -0.646355, 0.0198797, -0.024853, 0.00185134, -0.0509673, 0.0637179, 0.869616;
984  jacobian << 0.00329772, -0.194435, -0.112919,
985  -0.0115151, -0.0556828, -0.0323379,
986  0, 0.00993552, -0.0177924,
987  0.00454774, -0.267977, -0.155057,
988  -0.01588, -0.0767438, -0.0444055,
989  0, 0.0165184, 0.00951859,
990  -0.0119845, 0.706188, 0.414101,
991  0.0418479, 0.202239, 0.118591,
992  0, -0.0420476, 0.139591,
993  0.0579245, -0.143241, -0.0744983,
994  0.0647007, 0.128239, 0.066696,
995  0, -0.0728714, -0.0644042,
996  0.0786944, -0.18799, -0.100693,
997  0.0879002, 0.168302, 0.0901469,
998  0, -0.11798, -0.0656211,
999  -0.206536, 0.493387, 0.232834,
1000  -0.230697, -0.441714, -0.208449,
1001  0, 0.298475, 0.326197,
1002  0.098373, -0.124335, -0.0691047,
1003  0.0846586, 0.144477, 0.0802994,
1004  0, -0.110572, -0.0639689,
1005  0.129294, -0.151303, -0.0843603,
1006  0.111269, 0.175813, 0.0980263,
1007  0, -0.17058, -0.0955839,
1008  -0.358981, 0.420088, 0.230469,
1009  -0.308935, -0.488141, -0.267804,
1010  0, 0.457396, 0.270273,
1011  0.0531681, -0.159255, -0.085326,
1012  -0.0715067, -0.118412, -0.0634434,
1013  0, 0.0748349, 0.0556152,
1014  0.0715454, -0.207141, -0.112843,
1015  -0.0962226, -0.154018, -0.0839033,
1016  0, 0.119906, 0.0666997,
1017  -0.194671, 0.56362, 0.285766,
1018  0.261816, 0.419075, 0.212479,
1019  0, -0.315274, -0.273879,
1020  0.0178999, -0.122936, -0.0735487,
1021  0.014318, 0.153692, 0.0919485,
1022  0, -0.0190144, 0.0184454,
1023  0.024853, -0.170294, -0.100978,
1024  0.0198797, 0.212897, 0.12624,
1025  0, -0.0318257, -0.0186769,
1026  -0.0637179, 0.436598, 0.267206,
1027  -0.0509673, -0.545823, -0.334054,
1028  0, 0.0786625, -0.152426;
1029  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
1030  EXPECT_TRUE(test_jacobian(problem));
1031  }
1032  catch (const std::exception& e)
1033  {
1034  ADD_FAILURE() << "Uncaught exception! " << e.what();
1035  }
1036 }
1037 
1038 TEST(ExoticaTaskMaps, testPoint2Line)
1039 {
1040  try
1041  {
1042  TEST_COUT << "PointToLine Test";
1043  Initializer map("exotica/PointToLine", {{"Name", std::string("MyTask")},
1044  // {"EndPoint", std::string("0.5 0.5 1")},
1045  {"EndPoint", std::string("0.5 0.5 0")},
1046  {"EndEffector", std::vector<Initializer>(
1047  {Initializer("Frame", {{"Link", std::string("endeff")},
1048  {"LinkOffset", std::string("0.5 0 0.5")},
1049  {"Base", std::string("base")},
1050  {"BaseOffset", std::string("0.5 0.5 0")}})})}});
1051  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1052  EXPECT_TRUE(test_random(problem));
1053  // TODO: Add test_values
1054 
1055  EXPECT_TRUE(test_jacobian(problem));
1056  }
1057  catch (const std::exception& e)
1058  {
1059  ADD_FAILURE() << "Uncaught exception! " << e.what();
1060  }
1061 }
1062 
1063 TEST(ExoticaTaskMaps, testPoint2Plane)
1064 {
1065  try
1066  {
1067  {
1068  TEST_COUT << "PointToPlane Test - Align with world";
1069  Initializer map("exotica/PointToPlane", {{"Name", std::string("MyTask")},
1070  {"EndPoint", std::string("1 2 3")},
1071  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("endeff")}})})}});
1072  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1073  EXPECT_TRUE(test_random(problem));
1074  EXPECT_TRUE(test_jacobian(problem));
1075  EXPECT_TRUE(test_hessian(problem));
1076  }
1077 
1078  {
1079  TEST_COUT << "PointToPlane Test - Align with world with rotation";
1080  Initializer map("exotica/PointToPlane", {{"Name", std::string("MyTask")},
1081  {"EndPoint", std::string("1 2 3")},
1082  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("endeff")}, {"BaseOffset", std::string("0.1 0.1 0.1 0.7071 0 0.7071 0")}})})}});
1083  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1084  EXPECT_TRUE(test_random(problem));
1085  EXPECT_TRUE(test_jacobian(problem));
1086  EXPECT_TRUE(test_hessian(problem));
1087  }
1088  }
1089  catch (const std::exception& e)
1090  {
1091  ADD_FAILURE() << "Uncaught exception! " << e.what();
1092  }
1093 }
1094 
1095 TEST(ExoticaTaskMaps, testCollisionDistance)
1096 {
1097  try
1098  {
1099  TEST_COUT << "CollisionDistance test";
1100  Initializer map("exotica/CollisionDistance", {{"Name", std::string("MyTask")},
1101  {"CheckSelfCollision", true},
1102  {"WorldMargin", 0.0},
1103  {"RobotMargin", 0.0},
1104  {}});
1105  UnconstrainedEndPoseProblemPtr problem = setup_problem(map, "exotica/CollisionSceneFCLLatest");
1106  EXPECT_TRUE(test_random(problem));
1107  // EXPECT_TRUE(test_jacobian(problem, 1e-2)); // Currently failing
1108  }
1109  catch (const std::exception& e)
1110  {
1111  ADD_FAILURE() << "Uncaught exception! " << e.what();
1112  }
1113 }
1114 
1115 TEST(ExoticaTaskMaps, testSmoothCollisionDistance)
1116 {
1117  try
1118  {
1119  TEST_COUT << "SmoothCollisionDistance test";
1120  const std::vector<bool> linear_and_quadratic = {true, false};
1121  for (bool is_linear : linear_and_quadratic)
1122  {
1123  TEST_COUT << "Testing " << (is_linear ? "linear" : "quadratic") << " SmoothCollisionDistance";
1124  Initializer map("exotica/SmoothCollisionDistance", {{"Name", std::string("MyTask")},
1125  {"CheckSelfCollision", false},
1126  {"WorldMargin", 0.01},
1127  {"RobotMargin", 0.01},
1128  {"Linear", is_linear},
1129  {}});
1130  UnconstrainedEndPoseProblemPtr problem = setup_problem(map, "exotica/CollisionSceneFCLLatest");
1131  EXPECT_TRUE(test_random(problem));
1132  EXPECT_TRUE(test_jacobian(problem));
1133  }
1134  }
1135  catch (const std::exception& e)
1136  {
1137  ADD_FAILURE() << "Uncaught exception! " << e.what();
1138  }
1139 }
1140 
1141 TEST(ExoticaTaskMaps, testQuasiStatic)
1142 {
1143  try
1144  {
1145  {
1146  TEST_COUT << "QuasiStatic test inside capped";
1147  Initializer map("exotica/QuasiStatic", {
1148  {"Name", std::string("MyTask")},
1149  {"PositiveOnly", true},
1150  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-3 3 0")}}),
1151  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-3 -3 0")}}),
1152  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("3 -3 0")}}),
1153  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("3 3 0")}})})},
1154  });
1155  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1156  EXPECT_TRUE(test_random(problem));
1157  EXPECT_TRUE(test_jacobian(problem));
1158  }
1159 
1160  {
1161  TEST_COUT << "QuasiStatic test outside capped";
1162  Initializer map("exotica/QuasiStatic", {
1163  {"Name", std::string("MyTask")},
1164  {"PositiveOnly", true},
1165  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-11 1 0")}}),
1166  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-11 -1 0")}}),
1167  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-10 -1 0")}}),
1168  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-10 1 0")}})})},
1169  });
1170  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1171  EXPECT_TRUE(test_random(problem));
1172  EXPECT_TRUE(test_jacobian(problem));
1173  }
1174 
1175  {
1176  TEST_COUT << "QuasiStatic test inside";
1177  Initializer map("exotica/QuasiStatic", {
1178  {"Name", std::string("MyTask")},
1179  {"PositiveOnly", false},
1180  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-3 3 0")}}),
1181  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-3 -3 0")}}),
1182  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("3 -3 0")}}),
1183  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("3 3 0")}})})},
1184  });
1185  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1186  EXPECT_TRUE(test_random(problem));
1187  EXPECT_TRUE(test_jacobian(problem));
1188  }
1189 
1190  {
1191  TEST_COUT << "QuasiStatic test outside";
1192  Initializer map("exotica/QuasiStatic", {
1193  {"Name", std::string("MyTask")},
1194  {"PositiveOnly", false},
1195  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-11 1 0")}}),
1196  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-11 -1 0")}}),
1197  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-10 -1 0")}}),
1198  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-10 1 0")}})})},
1199  });
1200  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1201  EXPECT_TRUE(test_random(problem));
1202  EXPECT_TRUE(test_jacobian(problem));
1203  }
1204  }
1205  catch (const std::exception& e)
1206  {
1207  ADD_FAILURE() << "Uncaught exception! " << e.what();
1208  }
1209 }
1210 
1211 TEST(ExoticaTaskMaps, testJointSmoothingBackwardDifference)
1212 {
1213  try
1214  {
1215  std::vector<std::string> joint_smoothing_taskmaps = {"JointVelocityBackwardDifference", "JointAccelerationBackwardDifference", "JointJerkBackwardDifference"};
1216  for (const auto& smoothing_task : joint_smoothing_taskmaps)
1217  {
1218  {
1219  TEST_COUT << smoothing_task + " Test - test default initialisation";
1220  Initializer map("exotica/" + smoothing_task, {{"Name", std::string("MyTask")}, {"dt", 0.01}});
1221  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1222  EXPECT_TRUE(test_random(problem));
1223  EXPECT_TRUE(test_jacobian(problem));
1224  }
1225 
1226  // TODO(#437): Activate once solution for pointer casting/dynamic loading is found.
1227  // {
1228  // TEST_COUT << smoothing_task + " Test - test SetPreviousJointState initialisation";
1229  // Initializer map("exotica/" + smoothing_task, {{"Name", std::string("MyTask")}, {"dt", 0.01}});
1230  // UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1231  // Eigen::VectorXd q_rand(problem->N);
1232  // q_rand.setRandom();
1233  // if (smoothing_task == "JointVelocityBackwardDifference")
1234  // {
1235  // std::shared_ptr<TaskMap> my_task = problem->GetTaskMaps()["MyTask"];
1236  // std::shared_ptr<JointVelocityBackwardDifference> joint_velocity_smoothing_task = std::dynamic_pointer_cast<JointVelocityBackwardDifference>(my_task);
1237  // joint_velocity_smoothing_task->SetPreviousJointState(q_rand);
1238  // }
1239  // else if (smoothing_task == "JointAccelerationBackwardDifference")
1240  // {
1241  // std::shared_ptr<TaskMap> my_task = problem->GetTaskMaps()["MyTask"];
1242  // std::shared_ptr<JointAccelerationBackwardDifference> joint_acceleration_smoothing_task = std::dynamic_pointer_cast<JointAccelerationBackwardDifference>(my_task);
1243  // joint_acceleration_smoothing_task->SetPreviousJointState(q_rand);
1244  // }
1245  // else if (smoothing_task == "JointJerkBackwardDifference")
1246  // {
1247  // std::shared_ptr<TaskMap> my_task = problem->GetTaskMaps()["MyTask"];
1248  // std::shared_ptr<JointJerkBackwardDifference> joint_jerk_smoothing_task = std::dynamic_pointer_cast<JointJerkBackwardDifference>(my_task);
1249  // joint_jerk_smoothing_task->SetPreviousJointState(q_rand);
1250  // }
1251  // EXPECT_TRUE(test_random(problem));
1252  // EXPECT_TRUE(test_jacobian(problem));
1253  // }
1254  }
1255  }
1256  catch (const std::exception& e)
1257  {
1258  ADD_FAILURE() << "Uncaught exception! " << e.what();
1259  }
1260 }
1261 
1262 TEST(ExoticaTaskMaps, testAvoidLookAtSphere)
1263 {
1264  try
1265  {
1266  {
1267  TEST_COUT << "AvoidLookAtSphere";
1268 
1269  // Custom links
1270  std::vector<Initializer> custom_links({Initializer("Link", {{"Name", std::string("AvoidLookAtPosition")},
1271  {"Transform", std::string("1 1 2")}})});
1272 
1273  // Setup taskmap
1274  Initializer map("exotica/AvoidLookAtSphere",
1275  {{"Name", std::string("MyTask")},
1276  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("AvoidLookAtPosition")}, {"Base", std::string("")}})})},
1277  {"UseAsCost", std::string("1")},
1278  {"Radii", std::string("0.5")}});
1279  UnconstrainedEndPoseProblemPtr problem = setup_problem(map, "", custom_links);
1280  EXPECT_TRUE(test_random(problem));
1281  EXPECT_TRUE(test_jacobian(problem, 2e-5));
1282  }
1283  }
1284  catch (const std::exception& e)
1285  {
1286  ADD_FAILURE() << "Uncaught exception! " << e.what();
1287  }
1288 }
1289 
1290 TEST(ExoticaTaskMaps, testLookAt)
1291 {
1292  try
1293  {
1294  {
1295  TEST_COUT << "LookAt";
1296 
1297  // Custom links
1298  std::vector<Initializer> custom_links({Initializer("Link", {{"Name", std::string("LookAtTarget")},
1299  {"Transform", std::string("1 1 2")}})});
1300 
1301  // Setup taskmap
1302  Initializer map("exotica/LookAt", {{"Name", std::string("MyTask")},
1303  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("LookAtTarget")}, {"Base", std::string("endeff")}})})}});
1304  UnconstrainedEndPoseProblemPtr problem = setup_problem(map, "", custom_links);
1305  EXPECT_TRUE(test_random(problem));
1306  EXPECT_TRUE(test_jacobian(problem, 2e-5));
1307  }
1308  }
1309  catch (const std::exception& e)
1310  {
1311  ADD_FAILURE() << "Uncaught exception! " << e.what();
1312  }
1313 }
1314 
1315 TEST(ExoticaTaskMaps, testManipulability)
1316 {
1317  try
1318  {
1319  TEST_COUT << "Manipulability Test";
1320  Initializer map("exotica/Manipulability", {{"Name", std::string("MyTask")},
1321  {"EndEffector", std::vector<Initializer>(
1322  {Initializer("Frame", {{"Link", std::string("endeff")},
1323  {"LinkOffset", std::string("0.5 0 0.5")},
1324  {"Base", std::string("base")},
1325  {"BaseOffset", std::string("0.5 0.5 0")}})})}});
1326  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1327  EXPECT_TRUE(test_random(problem));
1328  // TODO: Add test_values
1329 
1330  EXPECT_TRUE(test_jacobian(problem));
1331  }
1332  catch (const std::exception& e)
1333  {
1334  ADD_FAILURE() << "Uncaught exception! " << e.what();
1335  }
1336 }
1337 
1338 int main(int argc, char** argv)
1339 {
1340  testing::InitGoogleTest(&argc, argv);
1341  ros::init(argc, argv, "EXOTica_test_maps");
1342  int ret = RUN_ALL_TESTS();
1343  Setup::Destroy();
1344  return ret;
1345 }
test_jacobian_time_indexed
bool test_jacobian_time_indexed(std::shared_ptr< T > problem, TimeIndexedTask &task, int t, const double eps=1e-5)
Definition: test_maps.cpp:204
main
int main(int argc, char **argv)
Definition: test_maps.cpp:1338
exotica_core.h
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
exotica::TimeIndexedTask::Phi
std::vector< TaskSpaceVector > Phi
ros.h
exotica::Server::Instance
static std::shared_ptr< Server > Instance()
exotica::Setup::CreateProblem
static std::shared_ptr< exotica::PlanningProblem > CreateProblem(const Initializer &init)
exotica::Setup::Destroy
static void Destroy()
exotica
Hessian
Eigen::Array< Eigen::MatrixXd, Eigen::Dynamic, 1 > Hessian
EXPECT_TRUE
#define EXPECT_TRUE(args)
setup_problem
UnconstrainedEndPoseProblemPtr setup_problem(Initializer &map, const std::string &collision_scene="", const std::vector< Initializer > &links=std::vector< Initializer >())
Definition: test_maps.cpp:239
test_values
bool test_values(Eigen::MatrixXdRefConst Xref, Eigen::MatrixXdRefConst Yref, Eigen::MatrixXdRefConst Jref, UnconstrainedEndPoseProblemPtr problem, const double eps=1e-5)
Definition: test_maps.cpp:81
y
const T & y
setup_time_indexed_problem
UnconstrainedTimeIndexedProblemPtr setup_time_indexed_problem(Initializer &map)
Definition: test_maps.cpp:278
TEST
TEST(ExoticaTaskMaps, testEffPositionXY)
Definition: test_maps.cpp:309
exotica::TaskSpaceVector
UnconstrainedEndPoseProblemPtr
std::shared_ptr< exotica::UnconstrainedEndPoseProblem > UnconstrainedEndPoseProblemPtr
exotica::eps
constexpr double eps
Definition: quasi_static.cpp:40
Eigen::MatrixXdRefConst
const typedef Eigen::Ref< const Eigen::MatrixXd > & MatrixXdRefConst
srdf_string_
static const std::string srdf_string_
Definition: test_maps.cpp:44
exotica::TimeIndexedTask
exotica::Initializer
UnconstrainedTimeIndexedProblemPtr
std::shared_ptr< exotica::UnconstrainedTimeIndexedProblem > UnconstrainedTimeIndexedProblemPtr
exotica::TimeIndexedTask::jacobian
std::vector< Eigen::MatrixXd > jacobian
num_trials_
constexpr int num_trials_
Definition: test_maps.cpp:47
urdf_string_
static const std::string urdf_string_
Definition: test_maps.cpp:43
test_helpers.h
test_jacobian
bool test_jacobian(UnconstrainedEndPoseProblemPtr problem, const double eps=1e-5)
Definition: test_maps.cpp:115
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr
x
double x
test_hessian
bool test_hessian(UnconstrainedEndPoseProblemPtr problem, const double eps=1e-5)
Definition: test_maps.cpp:156
print_debug_information_
constexpr bool print_debug_information_
Definition: test_maps.cpp:46
t
geometry_msgs::TransformStamped t
test_random
bool test_random(UnconstrainedEndPoseProblemPtr problem)
Definition: test_maps.cpp:49
TEST_COUT
#define TEST_COUT

exotica_core_task_maps
Author(s): Yiming Yang, Michael Camilleri
autogenerated on Fri Aug 2 2024 08:44:10