test_kinematics.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2020, University of Edinburgh
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without specific
15 // prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
30 #include <exotica_core/exotica_core.h>
31 #include <exotica_core/tools/test_helpers.h>
32 #include <gtest/gtest.h>
33 
34 using namespace exotica;
35 
36 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>";
37 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>";
38 
39 constexpr int num_trials_ = 100;
40 
41 class TestClass
42 {
43 public:
44  ScenePtr scene;
45  KinematicSolution solution;
46  int N;
47 
48  void UpdateKinematics(std::shared_ptr<KinematicResponse> response)
49  {
50  solution.Create(response);
51  }
52 
53  TestClass()
54  {
55  Server::Instance()->GetModel("robot_description", urdf_string_, srdf_string_);
56  scene.reset(new Scene());
57  Initializer init("Scene", {
58  {"Name", std::string("MyScene")},
59  {"JointGroup", std::string("arm")},
60  {"DoNotInstantiateCollisionScene", std::string("1")},
61  });
62  scene->InstantiateInternal(SceneInitializer(init));
63 
64  KinematicsRequest request;
65  request.flags = KIN_FK | KIN_J | KIN_H;
66  request.frames = {KinematicFrameRequest("endeff")};
67  solution = KinematicSolution(0, 1);
68  scene->RequestKinematics(request, std::bind(&TestClass::UpdateKinematics, this, std::placeholders::_1));
69 
70  N = scene->GetKinematicTree().GetNumControlledJoints();
71  }
72 };
73 
74 bool test_jacobian(TestClass& test, const double eps = 1e-5)
75 {
76  constexpr double h = 1e-5;
77 
78  TEST_COUT << "Testing Jacobian with h=" << h << ", eps=" << eps;
79  for (int k = 0; k < num_trials_; ++k)
80  {
81  Eigen::VectorXd x0(test.N);
82  x0 = test.scene->GetKinematicTree().GetRandomControlledState();
83  test.scene->Update(x0, 0.0);
84  KDL::Frame y0 = test.solution.Phi(0);
85  KDL::Jacobian J0 = test.solution.jacobian(0);
86  KDL::Jacobian jacobian(test.N);
87  jacobian.data.setZero();
88  for (int i = 0; i < test.N; ++i)
89  {
90  Eigen::VectorXd x(x0);
91  x(i) += h;
92  test.scene->Update(x, 0.0);
93  KDL::Frame y = test.solution.Phi(0);
94  KDL::Twist diff = KDL::diff(y0, y);
95  for (int j = 0; j < 6; ++j)
96  jacobian.data(j, i) = diff[j] / h;
97  }
98  double errJ = (jacobian.data - J0.data).norm();
99  if (errJ > eps)
100  {
101  TEST_COUT << "x: " << x0.transpose();
102  TEST_COUT << "J*:\n"
103  << jacobian.data;
104  TEST_COUT << "J:\n"
105  << J0.data;
106  TEST_COUT << "(J*-J):\n"
107  << (jacobian.data - J0.data);
108  ADD_FAILURE() << "Jacobian error out of bounds: " << errJ;
109  }
110  }
111  return true;
112 }
113 
114 bool test_hessian(TestClass& test, const double eps = 1e-5)
115 {
116  constexpr double h = 1e-5;
117 
118  TEST_COUT << "Testing Hessian with h=" << h << ", eps=" << eps;
119  for (int l = 0; l < num_trials_; ++l)
120  {
121  Eigen::VectorXd x0(test.N);
122  x0 = test.scene->GetKinematicTree().GetRandomControlledState();
123  test.scene->Update(x0, 0.0);
124  Hessian H0 = test.solution.hessian(0);
125  Hessian hessian = Hessian::Constant(6, Eigen::MatrixXd::Zero(test.N, test.N));
126  Eigen::VectorXd x;
127  for (int j = 0; j < test.N; ++j)
128  {
129  x = x0;
130  x(j) += h;
131  test.scene->Update(x, 0.0);
132  Eigen::MatrixXd J1 = test.solution.jacobian(0).data;
133  x = x0;
134  x(j) -= h;
135  test.scene->Update(x, 0.0);
136  Eigen::MatrixXd J2 = test.solution.jacobian(0).data;
137  for (int i = 0; i < test.N; ++i)
138  {
139  for (int k = 0; k < 6; ++k)
140  {
141  hessian(k)(i, j) = (J1(k, i) - J2(k, i)) / (2.0 * h);
142  }
143  }
144  }
145  double errH = 0;
146  for (int i = 0; i < hessian.rows(); ++i) errH += (hessian(i) - H0(i)).norm();
147  if (errH > eps)
148  {
149  TEST_COUT << "x: " << x0.transpose();
150  TEST_COUT << "H*:\n"
151  << hessian(0)
152  << "\n\n"
153  << hessian(1)
154  << "\n\n"
155  << hessian(2)
156  << "\n\n"
157  << hessian(3)
158  << "\n\n"
159  << hessian(4)
160  << "\n\n"
161  << hessian(5)
162  << "\n\n...";
163  TEST_COUT << "H:\n"
164  << H0(0)
165  << "\n\n"
166  << H0(1)
167  << "\n\n"
168  << H0(2)
169  << "\n\n"
170  << H0(3)
171  << "\n\n"
172  << H0(4)
173  << "\n\n"
174  << H0(5)
175  << "\n...";
176  ADD_FAILURE() << "Hessian error out of bounds: " << errH;
177  }
178  }
179  return true;
180 }
181 
182 TEST(ExoticaCore, testKinematicJacobian)
183 {
184  try
185  {
186  TEST_COUT << "Kinematic Jacobian test";
187  TestClass test;
188  EXPECT_TRUE(test_jacobian(test));
189  }
190  catch (const std::exception& e)
191  {
192  ADD_FAILURE() << "Uncaught exception! " << e.what();
193  }
194 }
195 
196 TEST(ExoticaCore, testKinematicHessian)
197 {
198  try
199  {
200  TEST_COUT << "Kinematic Hessian test";
201  TestClass test;
202  EXPECT_TRUE(test_hessian(test));
203  }
204  catch (const std::exception& e)
205  {
206  ADD_FAILURE() << "Uncaught exception! " << e.what();
207  }
208 }
209 
210 int main(int argc, char** argv)
211 {
212  testing::InitGoogleTest(&argc, argv);
213  int ret = RUN_ALL_TESTS();
214  Setup::Destroy();
215  return ret;
216 }
main
int main(int argc, char **argv)
Definition: test_kinematics.cpp:210
exotica::KinematicSolution::jacobian
Eigen::Map< ArrayJacobian > jacobian
Definition: kinematic_tree.h:138
KDL::diff
IMETHOD doubleVel diff(const doubleVel &a, const doubleVel &b, double dt=1.0)
exotica_core.h
exotica::KinematicSolution::Create
void Create(std::shared_ptr< KinematicResponse > solution)
Definition: kinematic_tree.cpp:81
exotica::KinematicsRequest::frames
std::vector< KinematicFrameRequest > frames
Definition: kinematic_tree.h:97
TestClass::UpdateKinematics
void UpdateKinematics(std::shared_ptr< KinematicResponse > response)
Definition: test_kinematics.cpp:48
diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
exotica::KinematicSolution::hessian
Eigen::Map< ArrayHessian > hessian
Definition: kinematic_tree.h:139
TestClass::solution
KinematicSolution solution
Definition: test_kinematics.cpp:45
exotica::ScenePtr
std::shared_ptr< Scene > ScenePtr
Definition: scene.h:246
exotica::Server::Instance
static std::shared_ptr< Server > Instance()
Get the server.
Definition: server.h:65
KDL::Jacobian::data
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 6, Eigen::Dynamic > data
exotica::Setup::Destroy
static void Destroy()
Definition: setup.cpp:42
exotica::KinematicsRequest::flags
KinematicRequestFlags flags
Definition: kinematic_tree.h:96
test_hessian
bool test_hessian(TestClass &test, const double eps=1e-5)
Definition: test_kinematics.cpp:114
exotica::Hessian
Eigen::Array< Eigen::MatrixXd, Eigen::Dynamic, 1 > Hessian
Definition: conversions.h:154
exotica::KinematicSolution
The KinematicSolution is created from - and maps into - a KinematicResponse.
Definition: kinematic_tree.h:127
exotica::KinematicSolution::Phi
Eigen::Map< ArrayFrame > Phi
Definition: kinematic_tree.h:136
num_trials_
constexpr int num_trials_
Definition: test_kinematics.cpp:39
exotica::Scene
The class of EXOTica Scene.
Definition: scene.h:69
TEST
TEST(ExoticaCore, testKinematicJacobian)
Definition: test_kinematics.cpp:182
urdf_string_
static const std::string urdf_string_
Definition: test_kinematics.cpp:36
srdf_string_
static const std::string srdf_string_
Definition: test_kinematics.cpp:37
TestClass::scene
ScenePtr scene
Definition: test_kinematics.cpp:44
test_helpers.h
test_jacobian
bool test_jacobian(TestClass &test, const double eps=1e-5)
Definition: test_kinematics.cpp:74
x
double x
EXPECT_TRUE
#define EXPECT_TRUE(args)
TEST_COUT
#define TEST_COUT
Definition: test_helpers.h:48
Eigen::y
const T & y
Definition: autodiff_scalar.h:644

exotica_core
Author(s): Yiming Yang, Michael Camilleri
autogenerated on Sat Apr 10 2021 02:34:49