explicit-relative-transformation.cc 13.1 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
// Copyright (c) 2017, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-core.
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core. If not, see <http://www.gnu.org/licenses/>.

#define BOOST_TEST_MODULE ExplicitRelativeTransformationTest
18
#include <pinocchio/fwd.hpp>
19
20
21
22
#include <boost/test/included/unit_test.hpp>

#include <boost/shared_ptr.hpp>

23
24
#include <../tests/util.hh>

25
26
27
28
29
// Force benchmark output
#define HPP_ENABLE_BENCHMARK 1
#include <hpp/util/timer.hh>

#include <hpp/core/fwd.hh>
30
#include <hpp/core/configuration-shooter/uniform.hh>
31
32
#include <hpp/constraints/explicit/relative-pose.hh>
#include <hpp/constraints/matrix-view.hh>
33
34
#include <hpp/constraints/differentiable-function.hh>
#include <hpp/constraints/generic-transformation.hh>
35
36
37
38
39

#include <hpp/pinocchio/simple-device.hh>
#include <hpp/pinocchio/urdf/util.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/configuration.hh>
40
#include <hpp/pinocchio/liegroup.hh>
41
42
43
44
45

using namespace hpp::pinocchio;
using namespace hpp::constraints;
using namespace hpp::core;

46
47
48
49
#define NB_RANDOM_CONF 2

const value_type test_precision = 1e-8;

50
51
DevicePtr_t createRobot ()
{
52
53
  //DevicePtr_t robot = unittest::makeDevice(unittest::HumanoidRomeo, "romeo");
  DevicePtr_t robot (Device::create ("2-objects"));
54
  urdf::loadModel (robot, 0, "obj1/", "freeflyer", "file://" DATA_DIR "/empty.urdf", "");
55
  robot->controlComputation((Computation_t) (JOINT_POSITION | JACOBIAN));
56
57
58
59
60
61
62
63
  robot->rootJoint()->lowerBound (0, -10);
  robot->rootJoint()->lowerBound (1, -10);
  robot->rootJoint()->lowerBound (2, -10);
  robot->rootJoint()->upperBound (0,  10);
  robot->rootJoint()->upperBound (1,  10);
  robot->rootJoint()->upperBound (2,  10);

  /// Add a freeflyer at the end.
64
  urdf::loadModel (robot, 0, "obj2/", "freeflyer", "file://" DATA_DIR "/empty.urdf", "");
65
66
67
68
69
70
71
  JointPtr_t rj = robot->getJointByName("obj2/root_joint");
  rj->lowerBound (0, -10);
  rj->lowerBound (1, -10);
  rj->lowerBound (2, -10);
  rj->upperBound (0,  10);
  rj->upperBound (1,  10);
  rj->upperBound (2,  10);
72
73
74
75
76
77
78

  return robot;
}

template <typename T> inline typename T::template NRowsBlockXpr<3>::Type trans(const Eigen::MatrixBase<T>& j) { return const_cast<Eigen::MatrixBase<T>&>(j).derived().template topRows   <3>(); }
template <typename T> inline typename T::template NRowsBlockXpr<3>::Type omega(const Eigen::MatrixBase<T>& j) { return const_cast<Eigen::MatrixBase<T>&>(j).derived().template bottomRows<3>(); }

79
BOOST_AUTO_TEST_CASE (two_freeflyers)
80
81
82
{
  DevicePtr_t robot = createRobot();
  BOOST_REQUIRE (robot);
Joseph Mirabel's avatar
Joseph Mirabel committed
83
  ConfigurationShooterPtr_t cs = configurationShooter::Uniform::create(robot);
84

85
86
  JointPtr_t object2 = robot->getJointByName("obj2/root_joint");
  JointPtr_t object1  = robot->getJointByName("obj1/root_joint");
87

88
89
  Transform3f M2inO2 (Transform3f::Identity());
  Transform3f M1inO1 (Transform3f::Identity());
90

91
92
93
94
  ExplicitPtr_t enm (explicit_::RelativePose::create
                     ("explicit_relative_transformation", robot, object1,
                      object2, M1inO1, M2inO2));
  DifferentiableFunctionPtr_t ert (enm->explicitFunction ());
95
96

  Configuration_t q     = robot->currentConfiguration (),
97
                  qrand = *cs->shoot(),
98
99
100
                  qout = qrand;

  // Check the output value
101
102
  Eigen::RowBlockIndices outConf (enm->outputConf());
  Eigen::RowBlockIndices  inConf (enm-> inputConf());
103

104
  // Compute position of object2 by solving explicit constraints
105
106
107
108
  LiegroupElement q_obj2 (ert->outputSpace ());
  vector_t q_obj1 (inConf.rview(qout).eval());
  ert->value (q_obj2, q_obj1);
  outConf.lview(qout) = q_obj2.vector ();
109

110
  // Test that at solution configuration, object2 and robot frames coincide.
111
112
113
  robot->currentConfiguration(qout);
  robot->computeForwardKinematics();
  Transform3f diff =
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
    M1inO1.inverse() * object1->currentTransformation().inverse()
    * object2->currentTransformation() * M2inO2;

  BOOST_CHECK (diff.isIdentity());

  // Check Jacobian of implicit numerical constraints by finite difference
  //
  value_type dt (1e-5);
  Configuration_t q0 (qout);
  vector_t v (robot->numberDof ());
  matrix_t J (6, enm->function ().inputDerivativeSize());
  LiegroupElement value0 (enm->function ().outputSpace ()),
    value (enm->function ().outputSpace ());
  enm->function ().value (value0, q0);
  enm->function ().jacobian (J, q0);
    std::cout << "J=" << std::endl << J << std::endl;
    std::cout << "q0=" << q0.transpose () << std::endl;
  // First at solution configuration
  for (size_type i=0; i<12; ++i) {
    // test canonical basis vectors
    v.setZero (); v [i] = 1;
    integrate (robot, q0, dt * v, q);
    enm->function ().value (value, q);
    vector_t df ((value - value0)/dt);
    vector_t Jdq (J * v);
    std::cout << "v=" << v.transpose () << std::endl;
    std::cout << "q=" << q.transpose () << std::endl;
    std::cout << "df=" << df.transpose () << std::endl;
    std::cout << "Jdq=" << Jdq.transpose () << std::endl;
    std::cout << "||Jdq - df ||=" << (df - Jdq).norm () << std::endl << std::endl;
    BOOST_CHECK ((df - Jdq).norm () < 1e-4);
  }
  // Second at random configurations
147
  for (size_type i=0; i<NB_RANDOM_CONF; ++i) {
148
    q0 = *cs->shoot();
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
    enm->function ().jacobian (J, q0);
    std::cout << "J=" << std::endl << J << std::endl;
    std::cout << "q0=" << q0.transpose () << std::endl;
    enm->function ().value (value0, q0);
    enm->function ().jacobian (J, q0);
    for (size_type j=0; j<12; ++j) {
      v.setZero (); v [j] = 1;
      std::cout << "v=" << v.transpose () << std::endl;
      integrate (robot, q0, dt * v, q);
      std::cout << "q=" << q.transpose () << std::endl;
      enm->function ().value (value, q);
      vector_t df ((value - value0)/dt);
      vector_t Jdq (J * v);
      std::cout << "df=" << df.transpose () << std::endl;
      std::cout << "Jdq=" << Jdq.transpose () << std::endl;
      std::cout << "||Jdq - df ||=" << (df - Jdq).norm () << std::endl << std::endl;
      BOOST_CHECK ((df - Jdq).norm () < 1e-4);
    }
    std::cout << std::endl;
  }
}

BOOST_AUTO_TEST_CASE (two_frames_on_freeflyer)
{
  DevicePtr_t robot = createRobot();
  BOOST_REQUIRE (robot);
Joseph Mirabel's avatar
Joseph Mirabel committed
175
  ConfigurationShooterPtr_t cs = configurationShooter::Uniform::create(robot);
176
177
178
179
180
181
182
183
184
185

  JointPtr_t object2 = robot->getJointByName("obj2/root_joint");
  JointPtr_t object1  = robot->getJointByName("obj1/root_joint");

  Transform3f M2inO2 (Transform3f::Random ());
  Transform3f M1inO1 (Transform3f::Random ());

  std::cout << "M2inO2=" << M2inO2 << std::endl;
  std::cout << "M1inO1=" << M1inO1 << std::endl;

186
187
188
189
  ExplicitPtr_t enm (explicit_::RelativePose::create
                     ("explicit_relative_transformation", robot, object1,
                      object2, M1inO1, M2inO2));
  DifferentiableFunctionPtr_t ert (enm->explicitFunction ());
190
191

  Configuration_t q     = robot->currentConfiguration (),
192
                  qrand = *cs->shoot(),
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
                  qout = qrand;

  // Check the output value
  Eigen::RowBlockIndices outConf (enm->outputConf());
  Eigen::RowBlockIndices  inConf (enm-> inputConf());

  // Compute position of object2 by solving explicit constraints
  LiegroupElement q_obj2 (ert->outputSpace ());
  vector_t q_obj1 (inConf.rview(qout).eval());
  ert->value (q_obj2, q_obj1);
  outConf.lview(qout) = q_obj2.vector ();

  // Test that at solution configuration, object2 and robot frames coincide.
  robot->currentConfiguration(qout);
  robot->computeForwardKinematics();
  Transform3f diff =
    M1inO1.inverse() * object1->currentTransformation().inverse()
    * object2->currentTransformation() * M2inO2;
211
212
213

  BOOST_CHECK (diff.isIdentity());

214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
  // Check Jacobian of implicit numerical constraints by finite difference
  //
  value_type dt (1e-5);
  Configuration_t q0 (qout);
  vector_t v (robot->numberDof ());
  matrix_t J (6, enm->function ().inputDerivativeSize());
  LiegroupElement value0 (enm->function ().outputSpace ()),
    value (enm->function ().outputSpace ());
  enm->function ().value (value0, q0);
  enm->function ().jacobian (J, q0);
    std::cout << "J=" << std::endl << J << std::endl;
    std::cout << "q0=" << q0.transpose () << std::endl;
  // First at solution configuration
  for (size_type i=0; i<12; ++i) {
    // test canonical basis vectors
    v.setZero (); v [i] = 1;
    integrate (robot, q0, dt * v, q);
    enm->function ().value (value, q);
    vector_t df ((value - value0)/dt);
    vector_t Jdq (J * v);
    std::cout << "v=" << v.transpose () << std::endl;
    std::cout << "q=" << q.transpose () << std::endl;
    std::cout << "df=" << df.transpose () << std::endl;
    std::cout << "Jdq=" << Jdq.transpose () << std::endl;
    std::cout << "||Jdq - df ||=" << (df - Jdq).norm () << std::endl << std::endl;
    BOOST_CHECK ((df - Jdq).norm () < 1e-4);
  }
  // Second at random configurations
242
  for (size_type i=0; i<NB_RANDOM_CONF; ++i) {
243
    q0 = *cs->shoot();
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
    enm->function ().jacobian (J, q0);
    std::cout << "J=" << std::endl << J << std::endl;
    std::cout << "q0=" << q0.transpose () << std::endl;
    enm->function ().value (value0, q0);
    enm->function ().jacobian (J, q0);
    for (size_type j=0; j<12; ++j) {
      v.setZero (); v [j] = 1;
      std::cout << "v=" << v.transpose () << std::endl;
      integrate (robot, q0, dt * v, q);
      std::cout << "q=" << q.transpose () << std::endl;
      enm->function ().value (value, q);
      vector_t df ((value - value0)/dt);
      vector_t Jdq (J * v);
      std::cout << "df=" << df.transpose () << std::endl;
      std::cout << "Jdq=" << Jdq.transpose () << std::endl;
      std::cout << "||Jdq - df ||=" << (df - Jdq).norm () << std::endl << std::endl;
      BOOST_CHECK ((df - Jdq).norm () < 1e-4);
    }
    std::cout << std::endl;
  }
264
}
265
266
267
268
269

BOOST_AUTO_TEST_CASE (compare_to_relative_transform)
{
  DevicePtr_t robot = createRobot();
  BOOST_REQUIRE (robot);
Joseph Mirabel's avatar
Joseph Mirabel committed
270
  ConfigurationShooterPtr_t cs = configurationShooter::Uniform::create(robot);
271
272
273
274
275
276
277
278
279
280

  JointPtr_t object2 = robot->getJointByName("obj2/root_joint");
  JointPtr_t object1  = robot->getJointByName("obj1/root_joint");

  Transform3f M2inO2 (Transform3f::Random ());
  Transform3f M1inO1 (Transform3f::Random ());

  BOOST_TEST_MESSAGE("M2inO2=" << M2inO2);
  BOOST_TEST_MESSAGE("M1inO1=" << M1inO1);

281
282
283
284
  ExplicitPtr_t enm (explicit_::RelativePose::create
                     ("explicit_relative_transformation", robot, object1,
                      object2, M1inO1, M2inO2));
  DifferentiableFunctionPtr_t ert (enm->explicitFunction ());
285
286
287
288
289
290
  DifferentiableFunctionPtr_t irt = enm->functionPtr();
  RelativeTransformation::Ptr_t rt = RelativeTransformation::create (
      "relative_transformation", robot,
      object1, object2, M1inO1, M2inO2);

  Configuration_t q     = robot->currentConfiguration (),
291
                  qrand = *cs->shoot(),
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
                  qout = qrand;

  // Check the output value
  Eigen::RowBlockIndices outConf (enm->outputConf());
  Eigen::RowBlockIndices  inConf (enm-> inputConf());

  // Compute position of object2 by solving explicit constraints
  LiegroupElement q_obj2 (ert->outputSpace ());
  vector_t q_obj1 (inConf.rview(qout).eval());
  ert->value (q_obj2, q_obj1);
  outConf.lview(qout) = q_obj2.vector ();

  // Test that at solution configuration, object2 and robot frames coincide.
  robot->currentConfiguration(qout);
  robot->computeForwardKinematics();
  Transform3f diff =
    M1inO1.inverse() * object1->currentTransformation().inverse()
    * object2->currentTransformation() * M2inO2;

  BOOST_CHECK (diff.isIdentity());

  // Check that value and Jacobian of relative transform from GenericTransformation
  // and from ExplicitRelativeTransformation are equal

  BOOST_CHECK_EQUAL (irt->inputSize(), rt->inputSize());
  BOOST_CHECK_EQUAL (irt->inputDerivativeSize(), rt->inputDerivativeSize());
  BOOST_CHECK_EQUAL (irt->outputSize(), rt->outputSize());
  BOOST_CHECK_EQUAL (irt->outputDerivativeSize(), rt->outputDerivativeSize());
  BOOST_CHECK_EQUAL (*irt->outputSpace(), *rt->outputSpace());

  Configuration_t q0 (qout);
  LiegroupElement v0 (rt->outputSpace ()), v1 (v0),
                  q_out (q_obj2), q_in  (q_obj2);
325
  matrix_t J0 (rt->outputSpace()->nv(), rt->inputDerivativeSize()), J1 (J0);
326
327
328
329
330
331
332
333
334
335
336
337

  irt->value    (v0, q0);
  irt->jacobian (J0, q0);
  rt ->value    (v1, q0);
  rt ->jacobian (J1, q0);

  BOOST_CHECK (v0.vector().isZero(test_precision));
  BOOST_CHECK (v1.vector().isZero(test_precision));
  EIGEN_IS_APPROX (J0, J1, test_precision);

  // Second at random configurations
  for (size_type i=0; i<NB_RANDOM_CONF; ++i) {
338
    q0 = *cs->shoot();
339
340
341
342
343
344
345
346
347

    irt->value    (v0, q0);
    irt->jacobian (J0, q0);
    rt ->value    (v1, q0);
    rt ->jacobian (J1, q0);
    EIGEN_VECTOR_IS_APPROX (v0.vector(), v1.vector(), test_precision);
    EIGEN_IS_APPROX (J0, J1, test_precision);
  }
}