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  catch (const std::exception& e)
558  {
559  ADD_FAILURE() << "Uncaught exception! " << e.what();
560  }
561 }
562 
563 TEST(ExoticaTaskMaps, testJointTorqueMinimizationProxy)
564 {
565  try
566  {
567  TEST_COUT << "Joint torque minimization proxy test";
568 
569  Initializer map("exotica/JointTorqueMinimizationProxy", {{"Name", std::string("MyTask")},
570  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("endeff")}})})}});
571  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
572  EXPECT_TRUE(test_random(problem));
573 
574  EXPECT_TRUE(test_jacobian(problem, 1.e-4));
575  }
576  catch (const std::exception& e)
577  {
578  ADD_FAILURE() << "Uncaught exception! " << e.what();
579  }
580 }
581 
582 /*TEST(ExoticaTaskMaps, testJointVelocityLimit)
583 {
584  try
585  {
586  TEST_COUT << "JointVelocityLimit test";
587 
588  std::vector<Initializer> maps;
589  maps.reserve(3);
590 
591  // Test default
592  {
593  Eigen::VectorXd qd_max = Eigen::VectorXd::Ones(1);
594  Initializer map("exotica/JointVelocityLimit", {{"Name", std::string("MyTask")},
595  {"dt", 0.1},
596  {"MaximumJointVelocity", qd_max},
597  {"SafePercentage", 0.0}});
598  maps.emplace_back(map);
599  }
600 
601  // Test safe percentage
602  {
603  Eigen::VectorXd qd_max = Eigen::VectorXd::Ones(1);
604  Initializer map("exotica/JointVelocityLimit", {{"Name", std::string("MyTask")},
605  {"dt", 0.1},
606  {"MaximumJointVelocity", qd_max},
607  {"SafePercentage", 0.1}});
608  maps.emplace_back(map);
609  }
610 
611  // Test different joint velocity initialisations (vector 1, vector N)
612  {
613  UnconstrainedTimeIndexedProblemPtr problem = setup_time_indexed_problem(maps[0]);
614  Eigen::VectorXd qd_max = Eigen::VectorXd::Ones(problem->N);
615  Initializer map("exotica/JointVelocityLimit", {{"Name", std::string("MyTask")},
616  {"dt", 0.1},
617  {"MaximumJointVelocity", qd_max},
618  {"SafePercentage", 0.0}});
619  maps.emplace_back(map);
620  }
621 
622  for (auto map : maps)
623  {
624  UnconstrainedTimeIndexedProblemPtr problem = setup_time_indexed_problem(map);
625  EXPECT_TRUE(test_random(problem));
626 
627  for (int t = 0; t < problem->GetT(); ++t)
628  {
629  problem->Update(problem->GetScene()->GetKinematicTree().GetRandomControlledState(), t);
630  }
631 
632  for (int t = 0; t < problem->GetT(); ++t)
633  {
634  EXPECT_TRUE(test_jacobian_time_indexed(problem, problem->cost, t, 1e-4));
635  }
636  }
637  }
638  catch (const std::exception& e)
639  {
640  ADD_FAILURE() << "JointVelocityLimit: Uncaught exception! " << e.what();
641  }
642 }*/
643 
644 TEST(ExoticaTaskMaps, testSphereCollision)
645 {
646  try
647  {
648  TEST_COUT << "Sphere collision test";
649  Initializer map("exotica/SphereCollision", {{"Name", std::string("MyTask")},
650  {"Precision", 1e-2},
651  {"ReferenceFrame", std::string("base")},
652  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("base")}, {"Radius", 0.3}, {"Group", std::string("base")}}),
653  Initializer("Frame", {{"Link", std::string("endeff")}, {"Radius", 0.3}, {"Group", std::string("eff")}})})}});
654  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
655  EXPECT_TRUE(test_random(problem));
656 
657  int N = problem->N;
658  int M = problem->length_Phi;
659  int L = 5;
660  Eigen::MatrixXd X(L, N);
661  Eigen::MatrixXd Y(L, M);
662  Eigen::MatrixXd jacobian(L * M, N);
663 
664  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;
665  Y << 1, 5.71023e-44, 1.83279e-110, 6.87352e-16, 7.26371e-45;
666  jacobian << 0, 0.431392, 0.449344,
667  0, 0.431735, 0.26014,
668  0, -0.234475, -0.0135658,
669  0, -0.349195, -0.447214,
670  0, -0.270171, -0.430172;
671  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
672 
673  // EXPECT_TRUE(test_jacobian(problem)); // Throws!
674  }
675  catch (const std::exception& e)
676  {
677  ADD_FAILURE() << "Uncaught exception! " << e.what();
678  }
679 }
680 
681 TEST(ExoticaTaskMaps, testJointPose)
682 {
683  try
684  {
685  TEST_COUT << "JointPose test";
686  Initializer map("exotica/JointPose", {{"Name", std::string("MyTask")}});
687  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
688  EXPECT_TRUE(test_random(problem));
689 
690  {
691  int N = problem->N;
692  int M = problem->length_Phi;
693  int L = 5;
694  Eigen::MatrixXd X(L, N);
695  Eigen::MatrixXd Y(L, M);
696  Eigen::MatrixXd jacobian(L * M, N);
697 
698  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;
699  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;
700  jacobian << 1, 0, 0,
701  0, 1, 0,
702  0, 0, 1,
703  1, 0, 0,
704  0, 1, 0,
705  0, 0, 1,
706  1, 0, 0,
707  0, 1, 0,
708  0, 0, 1,
709  1, 0, 0,
710  0, 1, 0,
711  0, 0, 1,
712  1, 0, 0,
713  0, 1, 0,
714  0, 0, 1;
715  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
716  }
717  EXPECT_TRUE(test_jacobian(problem));
718  EXPECT_TRUE(test_hessian(problem));
719 
720  TEST_COUT << "JointPose test with reference";
721  map = Initializer("exotica/JointPose", {{"Name", std::string("MyTask")},
722  {"JointRef", std::string("0.5 0.5 0.5")}});
723  problem = setup_problem(map);
724  EXPECT_TRUE(test_random(problem));
725 
726  {
727  int N = problem->N;
728  int M = problem->length_Phi;
729  int L = 5;
730  Eigen::MatrixXd X(L, N);
731  Eigen::MatrixXd Y(L, M);
732  Eigen::MatrixXd jacobian(L * M, N);
733 
734  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;
735  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;
736  jacobian << 1, 0, 0,
737  0, 1, 0,
738  0, 0, 1,
739  1, 0, 0,
740  0, 1, 0,
741  0, 0, 1,
742  1, 0, 0,
743  0, 1, 0,
744  0, 0, 1,
745  1, 0, 0,
746  0, 1, 0,
747  0, 0, 1,
748  1, 0, 0,
749  0, 1, 0,
750  0, 0, 1;
751  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
752  }
753  EXPECT_TRUE(test_jacobian(problem));
754  EXPECT_TRUE(test_hessian(problem));
755 
756  TEST_COUT << "JointPose test with subset of joints";
757  map = Initializer("exotica/JointPose", {{"Name", std::string("MyTask")},
758  {"JointMap", std::string("0")}});
759  problem = setup_problem(map);
760  EXPECT_TRUE(test_random(problem));
761 
762  {
763  int N = problem->N;
764  int M = problem->length_Phi;
765  int L = 5;
766  Eigen::MatrixXd X(L, N);
767  Eigen::MatrixXd Y(L, M);
768  Eigen::MatrixXd jacobian(L * M, N);
769 
770  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;
771  Y << 0.912937, 0.717353, -0.203127, 0.821944, 0.900505;
772  jacobian << 1, 0, 0,
773  1, 0, 0,
774  1, 0, 0,
775  1, 0, 0,
776  1, 0, 0;
777  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
778  }
779  EXPECT_TRUE(test_jacobian(problem));
780  }
781  catch (const std::exception& e)
782  {
783  ADD_FAILURE() << "JointPose: Uncaught exception!" << e.what();
784  }
785 }
786 
787 TEST(ExoticaTaskMaps, testCoM)
788 {
789  try
790  {
791  TEST_COUT << "CoM test";
792  Initializer map("exotica/CenterOfMass", {{"Name", std::string("MyTask")},
793  {"EnableZ", true}});
794  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
795  EXPECT_TRUE(test_random(problem));
796 
797  {
798  int N = problem->N;
799  int M = problem->length_Phi;
800  int L = 5;
801  Eigen::MatrixXd X(L, N);
802  Eigen::MatrixXd Y(L, M);
803  Eigen::MatrixXd jacobian(L * M, N);
804 
805  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;
806  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;
807  jacobian << 0.0234831, 0.455657, 0.200919,
808  0.081112, -0.131919, -0.0581688,
809  0, -0.0844429, -0.0565023,
810  0.000766568, 0.443462, 0.19642,
811  0.0080578, -0.0421882, -0.0186862,
812  0, -0.00809418, 0.0895227,
813  -0.00972105, 0.446309, 0.214617,
814  0.157569, 0.0275346, 0.0132406,
815  0, -0.157868, -0.0266211,
816  -0.176455, 0.219463, 0.0736575,
817  0.117213, 0.330384, 0.110885,
818  0, -0.211838, -0.170949,
819  0.0317596, 0.376061, 0.149235,
820  -0.0587457, 0.203309, 0.0806805,
821  0, 0.0667812, 0.134774;
822  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
823  }
824  EXPECT_TRUE(test_jacobian(problem));
825 
826  TEST_COUT << "CoM test with a subset of links";
827  map = Initializer("exotica/CenterOfMass", {{"Name", std::string("MyTask")},
828  {"EnableZ", true},
829  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("link2")}}),
830  Initializer("Frame", {{"Link", std::string("endeff")}})})}});
831  problem = setup_problem(map);
832  EXPECT_TRUE(test_random(problem));
833 
834  {
835  int N = problem->N;
836  int M = problem->length_Phi;
837  int L = 5;
838  Eigen::MatrixXd X(L, N);
839  Eigen::MatrixXd Y(L, M);
840  Eigen::MatrixXd jacobian(L * M, N);
841 
842  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;
843  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;
844  jacobian << 0.0517162, 0.443188, 0.254921,
845  -0.167532, 0.13681, 0.0786927,
846  0, 0.175333, 0.0666905,
847  0.0502684, 0.412954, 0.235481,
848  0.083533, -0.248507, -0.141708,
849  0, -0.0974919, -0.00961589,
850  -0.129477, 0.228967, 0.0468775,
851  -0.3632, -0.0816248, -0.0167114,
852  0, 0.385588, 0.270459,
853  0.048991, 0.441801, 0.257042,
854  -0.17971, 0.12044, 0.0700725,
855  0, 0.186268, 0.0681488,
856  -0.00672432, 0.373021, 0.240882,
857  0.314586, 0.00797337, 0.00514888,
858  0, -0.314658, -0.132569;
859  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
860  }
861  EXPECT_TRUE(test_jacobian(problem));
862 
863  TEST_COUT << "CoM test with projection on XY plane";
864  map = Initializer("exotica/CenterOfMass", {{"Name", std::string("MyTask")},
865  {"EnableZ", false}});
866  problem = setup_problem(map);
867  EXPECT_TRUE(test_random(problem));
868 
869  {
870  int N = problem->N;
871  int M = problem->length_Phi;
872  int L = 5;
873  Eigen::MatrixXd X(L, N);
874  Eigen::MatrixXd Y(L, M);
875  Eigen::MatrixXd jacobian(L * M, N);
876 
877  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;
878  Y << -0.0228141, -0.0083524, 0.0595937, -0.0245025, -0.392081, -0.0974653, 0.200868, 0.0830305, -0.232814, 0.0734919;
879  jacobian << 0.0083524, 0.453225, 0.202958,
880  -0.0228141, 0.165929, 0.0743046,
881  0.0245025, 0.420734, 0.195957,
882  0.0595937, -0.172988, -0.0805696,
883  0.0974653, 0.254325, 0.0971889,
884  -0.392081, 0.0632213, 0.0241597,
885  -0.0830305, 0.394279, 0.162599,
886  0.200868, 0.162978, 0.0672114,
887  -0.0734919, 0.394649, 0.189283,
888  -0.232814, -0.124578, -0.0597504;
889  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
890  }
891  EXPECT_TRUE(test_jacobian(problem));
892 
893  TEST_COUT << "CoM test with attached object";
894  map = Initializer("exotica/CenterOfMass", {{"Name", std::string("MyTask")},
895  {"EnableZ", true}});
896  problem = setup_problem(map);
897 
898  problem->GetScene()->AddObject("Payload", KDL::Frame(), "", shapes::ShapeConstPtr(nullptr), KDL::RigidBodyInertia(0.5));
899  problem->GetScene()->AttachObjectLocal("Payload", "endeff", KDL::Frame());
900  EXPECT_TRUE(test_random(problem));
901  {
902  int N = problem->N;
903  int M = problem->length_Phi;
904  int L = 5;
905  Eigen::MatrixXd X(L, N);
906  Eigen::MatrixXd Y(L, M);
907  Eigen::MatrixXd jacobian(L * M, N);
908 
909  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;
910  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;
911  jacobian << 0.397228, 0.111109, -0.0232142,
912  -0.391939, 0.112608, -0.0235274,
913  0, 0.558038, 0.32103,
914  -0.238097, 0.336815, 0.150781,
915  0.197788, 0.405457, 0.181509,
916  0, -0.309533, -0.220165,
917  -0.439301, 0.182119, 0.0740843,
918  0.289066, 0.276772, 0.112588,
919  0, -0.525875, -0.293238,
920  -0.0987601, 0.378744, 0.199373,
921  -0.4839, -0.0772987, -0.0406905,
922  0, 0.493875, 0.250495,
923  -0.00533025, 0.577691, 0.320061,
924  0.122905, 0.0250538, 0.0138807,
925  0, -0.123021, 0.0389986;
926  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
927  }
928  EXPECT_TRUE(test_jacobian(problem));
929  }
930  catch (const std::exception& e)
931  {
932  ADD_FAILURE() << "Uncaught exception! " << e.what();
933  }
934 }
935 
936 TEST(ExoticaTaskMaps, testContinuousJointPose)
937 {
938  try
939  {
940  TEST_COUT << "ContinuousJointPose test";
941  Initializer map("exotica/ContinuousJointPose", {{"Name", std::string("MyTask")}, {}});
942  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
943  EXPECT_TRUE(test_random(problem));
944  EXPECT_TRUE(test_jacobian(problem));
945  EXPECT_TRUE(test_hessian(problem));
946  }
947  catch (const std::exception& e)
948  {
949  ADD_FAILURE() << "Uncaught exception! " << e.what();
950  }
951 }
952 
953 TEST(ExoticaTaskMaps, testIMesh)
954 {
955  try
956  {
957  TEST_COUT << "Interaction mesh test";
958  Initializer map("exotica/InteractionMesh", {{"Name", std::string("MyTask")},
959  {"ReferenceFrame", std::string("base")},
960  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("base")}}),
961  Initializer("Frame", {{"Link", std::string("link2")}}),
962  Initializer("Frame", {{"Link", std::string("endeff")}})})}});
963  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
964  EXPECT_TRUE(test_random(problem));
965 
966  int N = problem->N;
967  int M = problem->length_Phi;
968  int L = 5;
969  Eigen::MatrixXd X(L, N);
970  Eigen::MatrixXd Y(L, M);
971  Eigen::MatrixXd jacobian(L * M, N);
972  jacobian.setZero();
973  Y.setZero();
974  X.setZero();
975 
976  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;
977  Y << -0.0115151, -0.00329772, -0.652131, -0.01588, -0.00454774, 0.000489, 0.0418479, 0.0119845, 0.906934,
978  0.0647007, -0.0579245, -0.635726, 0.0879002, -0.0786944, 0.0262201, -0.230697, 0.206536, 0.836692,
979  0.0846586, -0.098373, -0.626482, 0.111269, -0.129294, 0.0559699, -0.308935, 0.358981, 0.824647,
980  -0.0715067, -0.0531681, -0.641823, -0.0962226, -0.0715454, 0.0264905, 0.261816, 0.194671, 0.879061,
981  0.014318, -0.0178999, -0.646355, 0.0198797, -0.024853, 0.00185134, -0.0509673, 0.0637179, 0.869616;
982  jacobian << 0.00329772, -0.194435, -0.112919,
983  -0.0115151, -0.0556828, -0.0323379,
984  0, 0.00993552, -0.0177924,
985  0.00454774, -0.267977, -0.155057,
986  -0.01588, -0.0767438, -0.0444055,
987  0, 0.0165184, 0.00951859,
988  -0.0119845, 0.706188, 0.414101,
989  0.0418479, 0.202239, 0.118591,
990  0, -0.0420476, 0.139591,
991  0.0579245, -0.143241, -0.0744983,
992  0.0647007, 0.128239, 0.066696,
993  0, -0.0728714, -0.0644042,
994  0.0786944, -0.18799, -0.100693,
995  0.0879002, 0.168302, 0.0901469,
996  0, -0.11798, -0.0656211,
997  -0.206536, 0.493387, 0.232834,
998  -0.230697, -0.441714, -0.208449,
999  0, 0.298475, 0.326197,
1000  0.098373, -0.124335, -0.0691047,
1001  0.0846586, 0.144477, 0.0802994,
1002  0, -0.110572, -0.0639689,
1003  0.129294, -0.151303, -0.0843603,
1004  0.111269, 0.175813, 0.0980263,
1005  0, -0.17058, -0.0955839,
1006  -0.358981, 0.420088, 0.230469,
1007  -0.308935, -0.488141, -0.267804,
1008  0, 0.457396, 0.270273,
1009  0.0531681, -0.159255, -0.085326,
1010  -0.0715067, -0.118412, -0.0634434,
1011  0, 0.0748349, 0.0556152,
1012  0.0715454, -0.207141, -0.112843,
1013  -0.0962226, -0.154018, -0.0839033,
1014  0, 0.119906, 0.0666997,
1015  -0.194671, 0.56362, 0.285766,
1016  0.261816, 0.419075, 0.212479,
1017  0, -0.315274, -0.273879,
1018  0.0178999, -0.122936, -0.0735487,
1019  0.014318, 0.153692, 0.0919485,
1020  0, -0.0190144, 0.0184454,
1021  0.024853, -0.170294, -0.100978,
1022  0.0198797, 0.212897, 0.12624,
1023  0, -0.0318257, -0.0186769,
1024  -0.0637179, 0.436598, 0.267206,
1025  -0.0509673, -0.545823, -0.334054,
1026  0, 0.0786625, -0.152426;
1027  EXPECT_TRUE(test_values(X, Y, jacobian, problem));
1028  EXPECT_TRUE(test_jacobian(problem));
1029  }
1030  catch (const std::exception& e)
1031  {
1032  ADD_FAILURE() << "Uncaught exception! " << e.what();
1033  }
1034 }
1035 
1036 TEST(ExoticaTaskMaps, testPoint2Line)
1037 {
1038  try
1039  {
1040  TEST_COUT << "PointToLine Test";
1041  Initializer map("exotica/PointToLine", {{"Name", std::string("MyTask")},
1042  // {"EndPoint", std::string("0.5 0.5 1")},
1043  {"EndPoint", std::string("0.5 0.5 0")},
1044  {"EndEffector", std::vector<Initializer>(
1045  {Initializer("Frame", {{"Link", std::string("endeff")},
1046  {"LinkOffset", std::string("0.5 0 0.5")},
1047  {"Base", std::string("base")},
1048  {"BaseOffset", std::string("0.5 0.5 0")}})})}});
1049  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1050  EXPECT_TRUE(test_random(problem));
1051  // TODO: Add test_values
1052 
1053  EXPECT_TRUE(test_jacobian(problem));
1054  }
1055  catch (const std::exception& e)
1056  {
1057  ADD_FAILURE() << "Uncaught exception! " << e.what();
1058  }
1059 }
1060 
1061 TEST(ExoticaTaskMaps, testPoint2Plane)
1062 {
1063  try
1064  {
1065  {
1066  TEST_COUT << "PointToPlane Test - Align with world";
1067  Initializer map("exotica/PointToPlane", {{"Name", std::string("MyTask")},
1068  {"EndPoint", std::string("1 2 3")},
1069  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("endeff")}})})}});
1070  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1071  EXPECT_TRUE(test_random(problem));
1072  EXPECT_TRUE(test_jacobian(problem));
1073  EXPECT_TRUE(test_hessian(problem));
1074  }
1075 
1076  {
1077  TEST_COUT << "PointToPlane Test - Align with world with rotation";
1078  Initializer map("exotica/PointToPlane", {{"Name", std::string("MyTask")},
1079  {"EndPoint", std::string("1 2 3")},
1080  {"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")}})})}});
1081  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1082  EXPECT_TRUE(test_random(problem));
1083  EXPECT_TRUE(test_jacobian(problem));
1084  EXPECT_TRUE(test_hessian(problem));
1085  }
1086  }
1087  catch (const std::exception& e)
1088  {
1089  ADD_FAILURE() << "Uncaught exception! " << e.what();
1090  }
1091 }
1092 
1093 TEST(ExoticaTaskMaps, testCollisionDistance)
1094 {
1095  try
1096  {
1097  TEST_COUT << "CollisionDistance test";
1098  Initializer map("exotica/CollisionDistance", {{"Name", std::string("MyTask")},
1099  {"CheckSelfCollision", true},
1100  {"WorldMargin", 0.0},
1101  {"RobotMargin", 0.0},
1102  {}});
1103  UnconstrainedEndPoseProblemPtr problem = setup_problem(map, "exotica/CollisionSceneFCLLatest");
1104  EXPECT_TRUE(test_random(problem));
1105  // EXPECT_TRUE(test_jacobian(problem, 1e-2)); // Currently failing
1106  }
1107  catch (const std::exception& e)
1108  {
1109  ADD_FAILURE() << "Uncaught exception! " << e.what();
1110  }
1111 }
1112 
1113 TEST(ExoticaTaskMaps, testSmoothCollisionDistance)
1114 {
1115  try
1116  {
1117  TEST_COUT << "SmoothCollisionDistance test";
1118  const std::vector<bool> linear_and_quadratic = {true, false};
1119  for (bool is_linear : linear_and_quadratic)
1120  {
1121  TEST_COUT << "Testing " << (is_linear ? "linear" : "quadratic") << " SmoothCollisionDistance";
1122  Initializer map("exotica/SmoothCollisionDistance", {{"Name", std::string("MyTask")},
1123  {"CheckSelfCollision", false},
1124  {"WorldMargin", 0.01},
1125  {"RobotMargin", 0.01},
1126  {"Linear", is_linear},
1127  {}});
1128  UnconstrainedEndPoseProblemPtr problem = setup_problem(map, "exotica/CollisionSceneFCLLatest");
1129  EXPECT_TRUE(test_random(problem));
1130  EXPECT_TRUE(test_jacobian(problem));
1131  }
1132  }
1133  catch (const std::exception& e)
1134  {
1135  ADD_FAILURE() << "Uncaught exception! " << e.what();
1136  }
1137 }
1138 
1139 TEST(ExoticaTaskMaps, testQuasiStatic)
1140 {
1141  try
1142  {
1143  {
1144  TEST_COUT << "QuasiStatic test inside capped";
1145  Initializer map("exotica/QuasiStatic", {
1146  {"Name", std::string("MyTask")},
1147  {"PositiveOnly", true},
1148  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-3 3 0")}}),
1149  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-3 -3 0")}}),
1150  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("3 -3 0")}}),
1151  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("3 3 0")}})})},
1152  });
1153  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1154  EXPECT_TRUE(test_random(problem));
1155  EXPECT_TRUE(test_jacobian(problem));
1156  }
1157 
1158  {
1159  TEST_COUT << "QuasiStatic test outside capped";
1160  Initializer map("exotica/QuasiStatic", {
1161  {"Name", std::string("MyTask")},
1162  {"PositiveOnly", true},
1163  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-11 1 0")}}),
1164  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-11 -1 0")}}),
1165  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-10 -1 0")}}),
1166  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-10 1 0")}})})},
1167  });
1168  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1169  EXPECT_TRUE(test_random(problem));
1170  EXPECT_TRUE(test_jacobian(problem));
1171  }
1172 
1173  {
1174  TEST_COUT << "QuasiStatic test inside";
1175  Initializer map("exotica/QuasiStatic", {
1176  {"Name", std::string("MyTask")},
1177  {"PositiveOnly", false},
1178  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-3 3 0")}}),
1179  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-3 -3 0")}}),
1180  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("3 -3 0")}}),
1181  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("3 3 0")}})})},
1182  });
1183  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1184  EXPECT_TRUE(test_random(problem));
1185  EXPECT_TRUE(test_jacobian(problem));
1186  }
1187 
1188  {
1189  TEST_COUT << "QuasiStatic test outside";
1190  Initializer map("exotica/QuasiStatic", {
1191  {"Name", std::string("MyTask")},
1192  {"PositiveOnly", false},
1193  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-11 1 0")}}),
1194  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-11 -1 0")}}),
1195  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-10 -1 0")}}),
1196  Initializer("Frame", {{"Link", std::string("")}, {"LinkOffset", std::string("-10 1 0")}})})},
1197  });
1198  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1199  EXPECT_TRUE(test_random(problem));
1200  EXPECT_TRUE(test_jacobian(problem));
1201  }
1202  }
1203  catch (const std::exception& e)
1204  {
1205  ADD_FAILURE() << "Uncaught exception! " << e.what();
1206  }
1207 }
1208 
1209 TEST(ExoticaTaskMaps, testJointSmoothingBackwardDifference)
1210 {
1211  try
1212  {
1213  std::vector<std::string> joint_smoothing_taskmaps = {"JointVelocityBackwardDifference", "JointAccelerationBackwardDifference", "JointJerkBackwardDifference"};
1214  for (const auto& smoothing_task : joint_smoothing_taskmaps)
1215  {
1216  {
1217  TEST_COUT << smoothing_task + " Test - test default initialisation";
1218  Initializer map("exotica/" + smoothing_task, {{"Name", std::string("MyTask")}, {"dt", 0.01}});
1219  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1220  EXPECT_TRUE(test_random(problem));
1221  EXPECT_TRUE(test_jacobian(problem));
1222  }
1223 
1224  // TODO(#437): Activate once solution for pointer casting/dynamic loading is found.
1225  // {
1226  // TEST_COUT << smoothing_task + " Test - test SetPreviousJointState initialisation";
1227  // Initializer map("exotica/" + smoothing_task, {{"Name", std::string("MyTask")}, {"dt", 0.01}});
1228  // UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1229  // Eigen::VectorXd q_rand(problem->N);
1230  // q_rand.setRandom();
1231  // if (smoothing_task == "JointVelocityBackwardDifference")
1232  // {
1233  // std::shared_ptr<TaskMap> my_task = problem->GetTaskMaps()["MyTask"];
1234  // std::shared_ptr<JointVelocityBackwardDifference> joint_velocity_smoothing_task = std::dynamic_pointer_cast<JointVelocityBackwardDifference>(my_task);
1235  // joint_velocity_smoothing_task->SetPreviousJointState(q_rand);
1236  // }
1237  // else if (smoothing_task == "JointAccelerationBackwardDifference")
1238  // {
1239  // std::shared_ptr<TaskMap> my_task = problem->GetTaskMaps()["MyTask"];
1240  // std::shared_ptr<JointAccelerationBackwardDifference> joint_acceleration_smoothing_task = std::dynamic_pointer_cast<JointAccelerationBackwardDifference>(my_task);
1241  // joint_acceleration_smoothing_task->SetPreviousJointState(q_rand);
1242  // }
1243  // else if (smoothing_task == "JointJerkBackwardDifference")
1244  // {
1245  // std::shared_ptr<TaskMap> my_task = problem->GetTaskMaps()["MyTask"];
1246  // std::shared_ptr<JointJerkBackwardDifference> joint_jerk_smoothing_task = std::dynamic_pointer_cast<JointJerkBackwardDifference>(my_task);
1247  // joint_jerk_smoothing_task->SetPreviousJointState(q_rand);
1248  // }
1249  // EXPECT_TRUE(test_random(problem));
1250  // EXPECT_TRUE(test_jacobian(problem));
1251  // }
1252  }
1253  }
1254  catch (const std::exception& e)
1255  {
1256  ADD_FAILURE() << "Uncaught exception! " << e.what();
1257  }
1258 }
1259 
1260 TEST(ExoticaTaskMaps, testAvoidLookAtSphere)
1261 {
1262  try
1263  {
1264  {
1265  TEST_COUT << "AvoidLookAtSphere";
1266 
1267  // Custom links
1268  std::vector<Initializer> custom_links({Initializer("Link", {{"Name", std::string("AvoidLookAtPosition")},
1269  {"Transform", std::string("1 1 2")}})});
1270 
1271  // Setup taskmap
1272  Initializer map("exotica/AvoidLookAtSphere",
1273  {{"Name", std::string("MyTask")},
1274  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("AvoidLookAtPosition")}, {"Base", std::string("")}})})},
1275  {"UseAsCost", std::string("1")},
1276  {"Radii", std::string("0.5")}});
1277  UnconstrainedEndPoseProblemPtr problem = setup_problem(map, "", custom_links);
1278  EXPECT_TRUE(test_random(problem));
1279  EXPECT_TRUE(test_jacobian(problem, 2e-5));
1280  }
1281  }
1282  catch (const std::exception& e)
1283  {
1284  ADD_FAILURE() << "Uncaught exception! " << e.what();
1285  }
1286 }
1287 
1288 TEST(ExoticaTaskMaps, testLookAt)
1289 {
1290  try
1291  {
1292  {
1293  TEST_COUT << "LookAt";
1294 
1295  // Custom links
1296  std::vector<Initializer> custom_links({Initializer("Link", {{"Name", std::string("LookAtTarget")},
1297  {"Transform", std::string("1 1 2")}})});
1298 
1299  // Setup taskmap
1300  Initializer map("exotica/LookAt", {{"Name", std::string("MyTask")},
1301  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("LookAtTarget")}, {"Base", std::string("endeff")}})})}});
1302  UnconstrainedEndPoseProblemPtr problem = setup_problem(map, "", custom_links);
1303  EXPECT_TRUE(test_random(problem));
1304  EXPECT_TRUE(test_jacobian(problem, 2e-5));
1305  }
1306  }
1307  catch (const std::exception& e)
1308  {
1309  ADD_FAILURE() << "Uncaught exception! " << e.what();
1310  }
1311 }
1312 
1313 TEST(ExoticaTaskMaps, testManipulability)
1314 {
1315  try
1316  {
1317  TEST_COUT << "Manipulability Test";
1318  Initializer map("exotica/Manipulability", {{"Name", std::string("MyTask")},
1319  {"EndEffector", std::vector<Initializer>(
1320  {Initializer("Frame", {{"Link", std::string("endeff")},
1321  {"LinkOffset", std::string("0.5 0 0.5")},
1322  {"Base", std::string("base")},
1323  {"BaseOffset", std::string("0.5 0.5 0")}})})}});
1324  UnconstrainedEndPoseProblemPtr problem = setup_problem(map);
1325  EXPECT_TRUE(test_random(problem));
1326  // TODO: Add test_values
1327 
1328  EXPECT_TRUE(test_jacobian(problem));
1329  }
1330  catch (const std::exception& e)
1331  {
1332  ADD_FAILURE() << "Uncaught exception! " << e.what();
1333  }
1334 }
1335 
1336 int main(int argc, char** argv)
1337 {
1338  testing::InitGoogleTest(&argc, argv);
1339  ros::init(argc, argv, "EXOTica_test_maps");
1340  int ret = RUN_ALL_TESTS();
1341  Setup::Destroy();
1342  return ret;
1343 }
exotica_core.h
urdf_string_
static const std::string urdf_string_
Definition: test_maps.cpp:43
exotica::TaskSpaceVector::data
Eigen::VectorXd data
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
num_trials_
constexpr int num_trials_
Definition: test_maps.cpp:47
test_hessian
bool test_hessian(UnconstrainedEndPoseProblemPtr problem, const double eps=1e-5)
Definition: test_maps.cpp:156
test_jacobian
bool test_jacobian(UnconstrainedEndPoseProblemPtr problem, const double eps=1e-5)
Definition: test_maps.cpp:115
exotica::Server::Instance
static std::shared_ptr< Server > Instance()
test_random
bool test_random(UnconstrainedEndPoseProblemPtr problem)
Definition: test_maps.cpp:49
print_debug_information_
constexpr bool print_debug_information_
Definition: test_maps.cpp:46
exotica::Setup::Destroy
static void Destroy()
Eigen::MatrixXdRefConst
const Eigen::Ref< const Eigen::MatrixXd > & MatrixXdRefConst
main
int main(int argc, char **argv)
Definition: test_maps.cpp:1336
Hessian
Eigen::Array< Eigen::MatrixXd, Eigen::Dynamic, 1 > Hessian
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
UnconstrainedTimeIndexedProblemPtr
std::shared_ptr< exotica::UnconstrainedTimeIndexedProblem > UnconstrainedTimeIndexedProblemPtr
UnconstrainedEndPoseProblemPtr
std::shared_ptr< exotica::UnconstrainedEndPoseProblem > UnconstrainedEndPoseProblemPtr
exotica::eps
constexpr double eps
Definition: quasi_static.cpp:40
exotica::Setup::CreateProblem
static std::shared_ptr< exotica::PlanningProblem > CreateProblem(const std::string &type, bool prepend=true)
test_helpers.h
exotica::TimeIndexedTask::Phi
std::vector< TaskSpaceVector > Phi
exotica::TimeIndexedTask::jacobian
std::vector< Eigen::MatrixXd > jacobian
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
x
double x
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
EXPECT_TRUE
#define EXPECT_TRUE(args)
TEST_COUT
#define TEST_COUT
srdf_string_
static const std::string srdf_string_
Definition: test_maps.cpp:44
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
y
const T & y
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr

exotica_core_task_maps
Author(s): Yiming Yang, Michael Camilleri
autogenerated on Sat Apr 10 2021 02:36:09