generic-transformation.cc 26.5 KB
Newer Older
Joseph Mirabel's avatar
Joseph Mirabel committed
1
2
3
// Copyright (c) 2016, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
4
5
6
7
8
9
10
11
12
13
14

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
Joseph Mirabel's avatar
Joseph Mirabel committed
15
//
16
17
18
19
20
21
22
23
24
25
26
27
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
Joseph Mirabel's avatar
Joseph Mirabel committed
28

29
30
#define EIGEN_RUNTIME_NO_MALLOC

31
#include <hpp/constraints/explicit/relative-pose.hh>
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
32
#include <hpp/constraints/generic-transformation.hh>
33
#include <hpp/constraints/solver/by-substitution.hh>
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
34
#include <hpp/pinocchio/configuration.hh>
35
#include <hpp/pinocchio/device.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
36
#include <hpp/pinocchio/joint-collection.hh>
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
37
#include <hpp/pinocchio/joint.hh>
38
#include <hpp/pinocchio/serialization.hh>
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
39
#include <hpp/pinocchio/simple-device.hh>
40
#include <hpp/pinocchio/urdf/util.hh>
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
41
42
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <sstream>
43

Joseph Mirabel's avatar
Joseph Mirabel committed
44
45
46
47
48
#include "hpp/constraints/tools.hh"

#define BOOST_TEST_MODULE hpp_constraints
#include <stdlib.h>

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
49
50
51
#include <boost/test/included/unit_test.hpp>

using hpp::constraints::EqualToZero;
52
53
54
55
using hpp::pinocchio::Configuration_t;
using hpp::pinocchio::ConfigurationPtr_t;
using hpp::pinocchio::Device;
using hpp::pinocchio::DevicePtr_t;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
56
using hpp::pinocchio::JointIndex;
57
using hpp::pinocchio::JointPtr_t;
58
using hpp::pinocchio::LiegroupSpace;
59
using hpp::pinocchio::Transform3f;
Joseph Mirabel's avatar
Joseph Mirabel committed
60

61
62
using hpp::pinocchio::urdf::loadModelFromString;

Joseph Mirabel's avatar
Joseph Mirabel committed
63
64
using namespace hpp::constraints;

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
65
66
67
68
69
70
class BasicConfigurationShooter {
 public:
  BasicConfigurationShooter(const DevicePtr_t& robot) : robot_(robot) {}
  virtual ConfigurationPtr_t shoot() const {
    size_type extraDim = robot_->extraConfigSpace().dimension();
    size_type offset = robot_->configSize() - extraDim;
71

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
72
    Configuration_t config(robot_->configSize());
Joseph Mirabel's avatar
Joseph Mirabel committed
73
    config.head(offset) = ::pinocchio::randomConfiguration(robot_->model());
74
75

    // Shoot extra configuration variables
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
76
77
78
    for (size_type i = 0; i < extraDim; ++i) {
      value_type lower = robot_->extraConfigSpace().lower(i);
      value_type upper = robot_->extraConfigSpace().upper(i);
79
      value_type range = upper - lower;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
80
81
82
83
      if ((range < 0) || (range == std::numeric_limits<double>::infinity())) {
        std::ostringstream oss("Cannot uniformy sample extra config variable ");
        oss << i << ". min = " << lower << ", max = " << upper << std::endl;
        throw std::runtime_error(oss.str());
84
      }
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
85
      config[offset + i] = lower + (upper - lower) * rand() / RAND_MAX;
Joseph Mirabel's avatar
Joseph Mirabel committed
86
    }
87
    return hpp::make_shared<Configuration_t>(config);
Joseph Mirabel's avatar
Joseph Mirabel committed
88
  }
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
89
90

 private:
91
  DevicePtr_t robot_;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
92
};  // class BasicConfigurationShooter
Joseph Mirabel's avatar
Joseph Mirabel committed
93

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
94
BOOST_AUTO_TEST_CASE(print) {
Joseph Mirabel's avatar
Joseph Mirabel committed
95
96
  DevicePtr_t device = hpp::pinocchio::unittest::makeDevice(
      hpp::pinocchio::unittest::HumanoidSimple);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
97
98
99
100
  JointPtr_t ee1 = device->getJointByName("lleg5_joint"),
             ee2 = device->getJointByName("rleg5_joint");
  BOOST_REQUIRE(device);
  BasicConfigurationShooter cs(device);
101

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
102
103
104
105
  device->currentConfiguration(*cs.shoot());
  device->computeForwardKinematics();
  Transform3f tf1(ee1->currentTransformation());
  Transform3f tf2(ee2->currentTransformation());
Joseph Mirabel's avatar
Joseph Mirabel committed
106

107
  std::vector<DifferentiableFunctionPtr_t> functions;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
108
109
110
111
112
113
114
115
116
117
118
119
  functions.push_back(Orientation::create("Orientation", device, ee2, tf2));
  functions.push_back(Position::create("Position", device, ee2, tf2, tf1));
  functions.push_back(
      Transformation::create("Transformation", device, ee1, tf1));
  functions.push_back(RelativeOrientation::create("RelativeOrientation", device,
                                                  ee1, ee2, tf1));
  functions.push_back(
      RelativePosition::create("RelativePosition", device, ee1, ee2, tf1, tf2));
  functions.push_back(RelativeTransformation::create(
      "RelativeTransformation", device, ee1, ee2, tf1, tf2));

  Configuration_t q1 = *cs.shoot(), q2 = *cs.shoot();
120
  for (std::size_t i = 0; i < functions.size(); ++i) {
121
122
123
124
    DifferentiableFunctionPtr_t f = functions[i];

    std::cout << *f << std::endl;

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
125
126
    LiegroupElement v(f->outputSpace());
    matrix_t J(f->outputDerivativeSize(), f->inputDerivativeSize());
127

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
128
129
    f->value(v, q1);
    f->jacobian(J, q1);
Joseph Mirabel's avatar
Joseph Mirabel committed
130
131
132
    // TODO this is broken at the moment because of the introduction
    // of a multithreaded device.
    // Eigen::internal::set_is_malloc_allowed(false);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
133
134
    f->value(v, q2);
    f->jacobian(J, q2);
Joseph Mirabel's avatar
Joseph Mirabel committed
135
    // Eigen::internal::set_is_malloc_allowed(true);
136
  }
137
138

  // Check active parameters
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
139
140
141
142
143
144
145
  ArrayXb ap1 =
      Orientation::create("Orientation", device, ee1, tf1)->activeParameters();
  ArrayXb ap2 =
      Orientation::create("Orientation", device, ee2, tf2)->activeParameters();
  ArrayXb ap12 =
      RelativeOrientation::create("RelativeOrientation", device, ee1, ee2, tf1)
          ->activeParameters();
146

147
148
  ArrayXb not_ap1 = (ap1 == false);
  ArrayXb not_ap2 = (ap2 == false);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
149
  BOOST_CHECK((ap12 == ((not_ap1 && ap2) || (ap1 && not_ap2))).all());
150
151

  // Check active derivative parameters
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
152
153
154
155
156
157
158
  ap1 = Orientation::create("Orientation", device, ee1, tf1)
            ->activeDerivativeParameters();
  ap2 = Orientation::create("Orientation", device, ee2, tf2)
            ->activeDerivativeParameters();
  ap12 =
      RelativeOrientation::create("RelativeOrientation", device, ee1, ee2, tf1)
          ->activeDerivativeParameters();
159
160
161

  not_ap1 = (ap1 == false);
  not_ap2 = (ap2 == false);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
162
  BOOST_CHECK((ap12 == ((not_ap1 && ap2) || (ap1 && not_ap2))).all());
Joseph Mirabel's avatar
Joseph Mirabel committed
163
}
164

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
165
BOOST_AUTO_TEST_CASE(multithread) {
Joseph Mirabel's avatar
Joseph Mirabel committed
166
167
  DevicePtr_t device = hpp::pinocchio::unittest::makeDevice(
      hpp::pinocchio::unittest::HumanoidSimple);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
168
169
170
171
172
  device->numberDeviceData(4);
  JointPtr_t ee1 = device->getJointByName("lleg5_joint"),
             ee2 = device->getJointByName("rleg5_joint");
  BOOST_REQUIRE(device);
  BasicConfigurationShooter cs(device);
173

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
174
175
176
177
  device->currentConfiguration(*cs.shoot());
  device->computeForwardKinematics();
  Transform3f tf1(ee1->currentTransformation());
  Transform3f tf2(ee2->currentTransformation());
178
179

  std::vector<DifferentiableFunctionPtr_t> functions;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
  functions.push_back(Orientation::create("Orientation", device, ee2, tf2));
  functions.push_back(Position::create("Position", device, ee2, tf2, tf1));
  functions.push_back(
      Transformation::create("Transformation", device, ee1, tf1));
  functions.push_back(RelativeOrientation::create("RelativeOrientation", device,
                                                  ee1, ee2, tf1));
  functions.push_back(
      RelativePosition::create("RelativePosition", device, ee1, ee2, tf1, tf2));
  functions.push_back(RelativeTransformation::create(
      "RelativeTransformation", device, ee1, ee2, tf1, tf2));
  functions.push_back(RelativeOrientation::create("RelativeOrientation", device,
                                                  ee1, JointPtr_t(), tf1));
  functions.push_back(RelativePosition::create("RelativePosition", device, ee1,
                                               JointPtr_t(), tf1, tf2));
  functions.push_back(RelativeTransformation::create(
      "RelativeTransformation", device, ee1, JointPtr_t(), tf1, tf2));
196
197
198
199
200
201

  const int N = 10;
  Configuration_t q = *cs.shoot();
  for (std::size_t i = 0; i < functions.size(); ++i) {
    DifferentiableFunctionPtr_t f = functions[i];

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
202
203
204
    std::vector<LiegroupElement> vs(N, LiegroupElement(f->outputSpace()));
    std::vector<matrix_t> Js(
        N, matrix_t(f->outputDerivativeSize(), f->inputDerivativeSize()));
205
206
#pragma omp parallel for
    for (int j = 0; j < 10; ++j) {
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
207
208
      f->value(vs[j], q);
      f->jacobian(Js[j], q);
209
210
211
    }

    for (int j = 1; j < N; ++j) {
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
212
213
      BOOST_CHECK_EQUAL(vs[0].vector(), vs[j].vector());
      BOOST_CHECK_EQUAL(Js[0], Js[j]);
214
215
216
    }
  }
}
217

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
218
BOOST_AUTO_TEST_CASE(serialization) {
219
220
  DevicePtr_t device = hpp::pinocchio::unittest::makeDevice(
      hpp::pinocchio::unittest::HumanoidSimple);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
221
222
223
224
  device->numberDeviceData(4);
  JointPtr_t ee1 = device->getJointByName("lleg5_joint"),
             ee2 = device->getJointByName("rleg5_joint");
  BOOST_REQUIRE(device);
225

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
226
227
228
229
  device->currentConfiguration(device->neutralConfiguration());
  device->computeForwardKinematics();
  Transform3f tf1(ee1->currentTransformation());
  Transform3f tf2(ee2->currentTransformation());
230
231

  std::vector<DifferentiableFunctionPtr_t> functions;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
  functions.push_back(Orientation::create("Orientation", device, ee2, tf2));
  functions.push_back(Position::create("Position", device, ee2, tf2, tf1));
  functions.push_back(
      Transformation::create("Transformation", device, ee1, tf1));
  functions.push_back(RelativeOrientation::create("RelativeOrientation", device,
                                                  ee1, ee2, tf1));
  functions.push_back(
      RelativePosition::create("RelativePosition", device, ee1, ee2, tf1, tf2));
  functions.push_back(RelativeTransformation::create(
      "RelativeTransformation", device, ee1, ee2, tf1, tf2));
  functions.push_back(RelativeOrientation::create("RelativeOrientation", device,
                                                  ee1, JointPtr_t(), tf1));
  functions.push_back(RelativePosition::create("RelativePosition", device, ee1,
                                               JointPtr_t(), tf1, tf2));
  functions.push_back(RelativeTransformation::create(
      "RelativeTransformation", device, ee1, JointPtr_t(), tf1, tf2));
248
249
250
251
252
253
254
255

  for (std::size_t i = 0; i < functions.size(); ++i) {
    DifferentiableFunctionPtr_t f = functions[i];

    DifferentiableFunctionPtr_t f_restored;

    std::stringstream ss;
    {
256
257
      hpp::serialization::xml_oarchive oa(ss);
      oa.insert(device->name(), device.get());
258
259
260
261
      oa << boost::serialization::make_nvp("function", f);
    }

    {
262
263
      hpp::serialization::xml_iarchive ia(ss);
      ia.insert(device->name(), device.get());
264
265
266
267
268
269
270
271
272
273
      ia >> boost::serialization::make_nvp("function", f_restored);
    }

    std::ostringstream oss1, oss2;
    oss1 << *f;
    oss2 << *f_restored;

    BOOST_CHECK_EQUAL(oss1.str(), oss2.str());
  }
}
274

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
275
276
277
278
279
280
BOOST_AUTO_TEST_CASE(RelativeTransformation_R3xSO3) {
  const std::string model(
      "<robot name=\"box\">"
      "  <link name=\"baselink\">"
      "  </link>"
      "</robot>");
281
282
283
284
285
286
287
288
289
290
291
292

  DevicePtr_t robot(Device::create("two-freeflyers"));
  // Create two freeflying boxes
  loadModelFromString(robot, 0, "1/", "freeflyer", model, "");
  loadModelFromString(robot, 0, "2/", "freeflyer", model, "");
  BOOST_CHECK(robot->configSize() == 14);
  BOOST_CHECK(robot->numberDof() == 12);
  BOOST_CHECK(robot->nbJoints() == 2);
  JointPtr_t j1(robot->jointAt(0));
  JointPtr_t j2(robot->jointAt(1));

  // Set joint bounds
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
293
294
295
296
297
  for (std::size_t i = 0; i < 2; ++i) {
    vector_t l(7);
    l << -2, -2, -2, -1, -1, -1, -1;
    vector_t u(7);
    u << 2, 2, 2, 1, 1, 1, 1;
298
299
300
301
302
    robot->jointAt(i)->lowerBounds(l);
    robot->jointAt(i)->upperBounds(u);
  }
  // Create constraint
  //
303
304
  // Excerpt from romeo-placard benchmark
  // Joint1: romeo/LWristPitch
305
  // Frame in joint 1
306
307
  //   R = 0.7071067739978436073, 0.70710678837525142715, 0
  //       -2.2663502965461253728e-09,  2.2663502504650490188e-09, -1
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
308
309
310
311
  //       -0.70710678837525142715,
  //       0.70710677399784382935, 3.2051032938795742666e-09
  //   p = 0.099999999776482578762, -3.2051032222399330291e-11,
  //   -0.029999999776482582509
312
  // Joint2: placard/root_joint
313
314
315
316
  // Frame in joint 2
  //   R =   1, 0, 0
  //         0, 1, 0
  //         0, 0, 1
317
318
319
  //   p = 0, 0, -0.34999999403953552246
  // mask: 1, 1, 1, 1, 1, 1,
  // Rhs: 0, 0, 0, 0, 0, 0, 1
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
320
  // active rows: [ 0, 5],
321
322
323

  matrix3_t R1, R2;
  vector3_t p1, p2;
324
  R1 << 0.7071067739978436073, 0.70710678837525142715, 0,
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
325
326
327
      -2.2663502965461253728e-09, 2.2663502504650490188e-09, -1,
      -0.70710678837525142715, 0.70710677399784382935,
      3.2051032938795742666e-09;
328
  p1 << 0.099999999776482578762, -3.2051032222399330291e-11,
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
329
      -0.029999999776482582509;
330
  R2.setIdentity();
331
  p2 << 0, 0, -0.34999999403953552246;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
332
  Transform3f tf1(R1, p1), tf2(R2, p2);
333
  std::vector<bool> mask = {false, false, false, false, false, true};
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
334
335
336
337
338
  ImplicitPtr_t constraint(Implicit::create(
      RelativeTransformationR3xSO3::create("RelativeTransformationR3xSO3",
                                           robot, j1, j2, tf1, tf2),
      6 * Equality, mask));
  BasicConfigurationShooter cs(robot);
339
340
341
  solver::BySubstitution solver(robot->configSpace());
  solver.errorThreshold(1e-10);
  solver.add(constraint);
342
343
344
  // Check that after setting right hand side with a configuration
  // the configuration satisfies the constraint since comparison type is
  // Equality.
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
345
  for (size_type i = 0; i < 1000; ++i) {
346
347
348
349
    ConfigurationPtr_t q(cs.shoot());
    vector6_t error;
    solver.rightHandSideFromConfig(*q);
    BOOST_CHECK(solver.isSatisfied(*q, error));
350
351
352
353
  }

  // Create grasp constraint with one degree of freedom in rotation along z
  mask = {true, true, true, true, true, false};
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
354
355
356
357
  ImplicitPtr_t c1(Implicit::create(
      RelativeTransformationR3xSO3::create("RelativeTransformationR3xSO3",
                                           robot, j1, j2, tf1, tf2),
      6 * EqualToZero, mask));
358
359
360
361
  solver::BySubstitution s1(robot->configSpace());
  s1.errorThreshold(1e-10);
  s1.add(c1);
  // Create grasp + complement as an explicit constraint
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
362
363
364
  ExplicitPtr_t c2(
      explicit_::RelativePose::create("ExplicitRelativePose", robot, j1, j2,
                                      tf1, tf2, 5 * EqualToZero << Equality));
365
366
367
368
  solver::BySubstitution s2(robot->configSpace());
  s2.errorThreshold(1e-4);
  s2.add(c2);

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
369
  for (size_type i = 0; i < 0; ++i) {
370
371
    ConfigurationPtr_t q_near(cs.shoot());
    ConfigurationPtr_t q_new(cs.shoot());
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
372
    if (i == 0) {
373
374
375
376
377
378
      // These configuration reproduce a numerical issue encountered with
      // benhmark romeo-placard.
      // If computation was exact, any configuration satisfying c2 should
      // satisfy c1.
      // Configuration q_new below satisfies c2 but not c1.
      *q_near << 0.18006349590534418, 0.3627623741970175, 0.9567759630330663,
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
379
380
381
382
          0.044416054309488175, 0.31532356328825556, 0.4604329042168087,
          0.8286131819306576, 0.45813483973344404, 0.23514459283216355,
          0.7573015903787429, 0.8141495491430896, 0.1383820163733335,
          0.3806970356973106, 0.4160296818567576;
383
      *q_new << 0.16026892741853033, 0.33925098736439646, 0.8976880203169203,
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
384
385
386
387
          -0.040130835169737825, 0.37473431876437147, 0.4405275981290593,
          0.8148000624051422, 0.43787674119234027, 0.18316291571416676,
          0.7189377922181226, 0.7699579340925136, 0.1989432638510445,
          0.35960786236482944, 0.4881275886709128;
388
389
390
391
392
393
394
395
396
397
398
399
400
401
    }
    s2.rightHandSideFromConfig(*q_near);
    vector6_t error;
    BOOST_CHECK(s1.isSatisfied(*q_near, error));
    hppDout(info, error);
    BOOST_CHECK(s2.isSatisfied(*q_near, error));
    hppDout(info, error);
    BOOST_CHECK(s1.isSatisfied(*q_new, error));
    hppDout(info, error);
    BOOST_CHECK(s2.isSatisfied(*q_new, error));
    hppDout(info, error);

    hppDout(info, s1);
    hppDout(info, s2);
402
403
  }
}
404

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
405
BOOST_AUTO_TEST_CASE(equality) {
406
407
  DevicePtr_t device = hpp::pinocchio::unittest::makeDevice(
      hpp::pinocchio::unittest::HumanoidSimple);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
408
409
410
411
  JointPtr_t ee1 = device->getJointByName("lleg5_joint"),
             ee2 = device->getJointByName("rleg5_joint");
  BOOST_REQUIRE(device);
  BasicConfigurationShooter cs(device);
412

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
413
414
415
416
  device->currentConfiguration(*cs.shoot());
  device->computeForwardKinematics();
  Transform3f tf1(ee1->currentTransformation());
  Transform3f tf2(ee2->currentTransformation());
417
418

  std::vector<DifferentiableFunctionPtr_t> functions;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
  functions.push_back(Orientation::create("Orientation", device, ee2, tf2));
  functions.push_back(RelativeTransformation::create("Orientation", device, ee1,
                                                     ee2, tf1, tf2));
  functions.push_back(RelativeTransformation::create(
      "RelativeTransformation", device, ee1, ee2, tf1, tf2));
  functions.push_back(RelativeTransformation::create(
      "RelativeTransformation", device, ee1, ee2, tf1, tf2));
  functions.push_back(RelativeTransformation::create(
      "othername_____________", device, ee1, ee2, tf1, tf2));
  // functions[2] and functions[3] are meant to have the same value with
  // different pointers

  BOOST_CHECK(functions[2].get() !=
              functions[3].get());  // boost implementation for ==
  BOOST_CHECK(*functions[2] ==
              *functions[3]);  // uses operator== defined in DiffFunc
  BOOST_CHECK(*functions[0] != *functions[2]);  // a lot of things are different
  BOOST_CHECK(*functions[2] != *functions[4]);  // only the names are different
  BOOST_CHECK(*functions[0] != *functions[1]);  // only the names are equal
Le Quang Anh's avatar
Le Quang Anh committed
438
439
}

440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
/* Create a robot with the following kinematic chain. All joints are
   translations along x.

                               universe
                                  |Px
                               test_x
                             /Px       \Px
                       joint_a0       joint_b0
                           |Px            |Px
                       joint_a1       joint_b1
                                          |FF
                                      joint_b2

*/
DevicePtr_t createRobot ()
{
  std::string urdf ("<robot name='test'>"
      "<link name='base_link'/>"
      "<link name='link_test_x'/>"
      "<joint name='test_x' type='prismatic'>"
        "<parent link='base_link'/>"
        "<child  link='link_test_x'/>"
        "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
      "</joint>"

      "<link name='link_a0'/>"
      "<link name='link_a1'/>"
      "<joint name='joint_a0' type='prismatic'>"
        "<parent link='link_test_x'/>"
        "<child  link='link_a0'/>"
        "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
      "</joint>"
      "<joint name='joint_a1' type='prismatic'>"
        "<parent link='link_a0'/>"
        "<child  link='link_a1'/>"
        "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
      "</joint>"

      "<link name='link_b0'/>"
      "<link name='link_b1'/>"
      "<link name='link_b2'/>"
      "<joint name='joint_b0' type='prismatic'>"
        "<parent link='link_test_x'/>"
        "<child  link='link_b0'/>"
        "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
      "</joint>"
      "<joint name='joint_b1' type='prismatic'>"
        "<parent link='link_b0'/>"
        "<child  link='link_b1'/>"
        "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
      "</joint>"
      "<joint name='joint_b2' type='floating'>"
        "<parent link='link_b1'/>"
        "<child  link='link_b2'/>"
      "</joint>"

      "</robot>"
      );


  DevicePtr_t robot = Device::create ("test");
  loadModelFromString (robot, 0, "", "anchor", urdf, "");
  return robot;
}

BOOST_AUTO_TEST_CASE (dependsOnRelPoseBetween) {
  DevicePtr_t device = createRobot();
  BOOST_REQUIRE (device);

  JointPtr_t ee1 = device->getJointByName ("joint_a1"),
             ee2 = device->getJointByName ("joint_b2");
  BOOST_REQUIRE (device);
Le Quang Anh's avatar
Le Quang Anh committed
512
  // ensure that the joint indices are as expected
513
  BOOST_REQUIRE (ee1->index() < ee2->index());
Le Quang Anh's avatar
Le Quang Anh committed
514

515
516
517
518
  device->currentConfiguration (device->neutralConfiguration());
  device->computeForwardKinematics ();
  Transform3f tf1 (ee1->currentTransformation ());
  Transform3f tf2 (ee2->currentTransformation ());
Le Quang Anh's avatar
Le Quang Anh committed
519
520
521
522
523
524

  DifferentiableFunctionPtr_t function;
  std::pair<JointConstPtr_t, JointConstPtr_t> joints;
  std::pair<JointConstPtr_t, JointConstPtr_t> jointsConstrained;
  ImplicitPtr_t constraint;

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
525
526
527
528
  function = Orientation::create("Orientation", device, ee2, tf2);
  joints = function->dependsOnRelPoseBetween(nullptr);
  BOOST_CHECK(!joints.first);
  BOOST_CHECK_EQUAL(joints.second->index(), ee2->index());
Le Quang Anh's avatar
Le Quang Anh committed
529
530
  // constraint does not fully constrain the relative pose
  // since it only involves the orientation
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
531
532
533
  constraint = Implicit::create(
      function, ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
      std::vector<bool>(function->outputSpace()->nv(), true));
Le Quang Anh's avatar
Le Quang Anh committed
534
  jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
535
536
  BOOST_CHECK(!jointsConstrained.first);
  BOOST_CHECK(!jointsConstrained.second);
Le Quang Anh's avatar
Le Quang Anh committed
537

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
538
539
540
541
  function = Position::create("Position", device, ee2, tf2, tf1);
  joints = function->dependsOnRelPoseBetween(nullptr);
  BOOST_CHECK(!joints.first);
  BOOST_CHECK_EQUAL(joints.second->index(), ee2->index());
Le Quang Anh's avatar
Le Quang Anh committed
542
543
  // constraint does not fully constrain the relative pose
  // since it only involves the position
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
544
545
546
  constraint = Implicit::create(
      function, ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
      std::vector<bool>(function->outputSpace()->nv(), true));
Le Quang Anh's avatar
Le Quang Anh committed
547
  jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
548
549
  BOOST_CHECK(!jointsConstrained.first);
  BOOST_CHECK(!jointsConstrained.second);
Le Quang Anh's avatar
Le Quang Anh committed
550

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
551
552
553
554
  function = Transformation::create("Transformation", device, ee1, tf1);
  joints = function->dependsOnRelPoseBetween(nullptr);
  BOOST_CHECK(!joints.first);
  BOOST_CHECK_EQUAL(joints.second->index(), ee1->index());
Le Quang Anh's avatar
Le Quang Anh committed
555
556
  // constraint does not fully constrain the relative pose
  // since the mask is not full
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
557
558
559
  constraint = Implicit::create(
      function, ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
      std::vector<bool>(function->outputSpace()->nv(), false));
Le Quang Anh's avatar
Le Quang Anh committed
560
  jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
561
562
  BOOST_CHECK(!jointsConstrained.first);
  BOOST_CHECK(!jointsConstrained.second);
Le Quang Anh's avatar
Le Quang Anh committed
563
  // constraint fully constrains the relative pose
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
564
565
566
  constraint = Implicit::create(
      function, ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
      std::vector<bool>(function->outputSpace()->nv(), true));
Le Quang Anh's avatar
Le Quang Anh committed
567
  jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
568
569
570
571
572
573
574
575
  BOOST_CHECK(!jointsConstrained.first);
  BOOST_CHECK_EQUAL(jointsConstrained.second->index(), ee1->index());

  function =
      RelativeOrientation::create("RelativeOrientation", device, ee1, ee2, tf1);
  joints = function->dependsOnRelPoseBetween(nullptr);
  BOOST_CHECK_EQUAL(joints.first->index(), ee1->index());
  BOOST_CHECK_EQUAL(joints.second->index(), ee2->index());
Le Quang Anh's avatar
Le Quang Anh committed
576
  // constraint does not fully constrain the relative pose
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
577
578
579
  constraint = Implicit::create(
      function, ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
      std::vector<bool>(function->outputSpace()->nv(), true));
Le Quang Anh's avatar
Le Quang Anh committed
580
  jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
581
582
583
584
585
586
587
588
  BOOST_CHECK(!jointsConstrained.first);
  BOOST_CHECK(!jointsConstrained.second);

  function =
      RelativePosition::create("RelativePosition", device, ee1, ee2, tf1, tf2);
  joints = function->dependsOnRelPoseBetween(nullptr);
  BOOST_CHECK_EQUAL(joints.first->index(), ee1->index());
  BOOST_CHECK_EQUAL(joints.second->index(), ee2->index());
Le Quang Anh's avatar
Le Quang Anh committed
589
  // constraint does not fully constrain the relative pose
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
590
591
592
  constraint = Implicit::create(
      function, ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
      std::vector<bool>(function->outputSpace()->nv(), true));
Le Quang Anh's avatar
Le Quang Anh committed
593
  jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
594
595
596
597
598
599
600
601
  BOOST_CHECK(!jointsConstrained.first);
  BOOST_CHECK(!jointsConstrained.second);

  function = RelativeTransformation::create("RelativeTransformation", device,
                                            ee1, ee2, tf1, tf2);
  joints = function->dependsOnRelPoseBetween(nullptr);
  BOOST_CHECK_EQUAL(joints.first->index(), ee1->index());
  BOOST_CHECK_EQUAL(joints.second->index(), ee2->index());
Le Quang Anh's avatar
Le Quang Anh committed
602
603
  // constraint does not fully constrain the relative pose
  // since mask is not full
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
604
605
606
  constraint = Implicit::create(
      function, ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
      std::vector<bool>(function->outputSpace()->nv(), false));
Le Quang Anh's avatar
Le Quang Anh committed
607
  jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
608
609
  BOOST_CHECK(!jointsConstrained.first);
  BOOST_CHECK(!jointsConstrained.second);
Le Quang Anh's avatar
Le Quang Anh committed
610
  // constraint fully constrains the relative pose
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
611
612
613
  constraint = Implicit::create(
      function, ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
      std::vector<bool>(function->outputSpace()->nv(), true));
Le Quang Anh's avatar
Le Quang Anh committed
614
  jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
  BOOST_CHECK_EQUAL(jointsConstrained.first->index(), ee1->index());
  BOOST_CHECK_EQUAL(jointsConstrained.second->index(), ee2->index());

  function = RelativeOrientation::create("RelativeOrientation", device, ee1,
                                         JointPtr_t(), tf1);
  joints = function->dependsOnRelPoseBetween(nullptr);
  BOOST_CHECK(!joints.first);
  BOOST_CHECK_EQUAL(joints.second->index(), ee1->index());

  function = RelativePosition::create("RelativePosition", device, ee1,
                                      JointPtr_t(), tf1, tf2);
  joints = function->dependsOnRelPoseBetween(nullptr);
  BOOST_CHECK(!joints.first);
  BOOST_CHECK_EQUAL(joints.second->index(), ee1->index());

  function = RelativeTransformationR3xSO3::create(
      "RelativeTransformationR3xSO3", device, ee1, JointPtr_t(), tf1, tf2);
  joints = function->dependsOnRelPoseBetween(nullptr);
  BOOST_CHECK(!joints.first);
  BOOST_CHECK_EQUAL(joints.second->index(), ee1->index());
Le Quang Anh's avatar
Le Quang Anh committed
635
  // constraint fully constrains the relative pose
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
636
637
638
  constraint = Implicit::create(
      function, ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
      std::vector<bool>(function->outputSpace()->nv(), true));
Le Quang Anh's avatar
Le Quang Anh committed
639
  jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
640
641
  BOOST_CHECK(!jointsConstrained.first);
  BOOST_CHECK_EQUAL(jointsConstrained.second->index(), ee1->index());
Le Quang Anh's avatar
Le Quang Anh committed
642
643

  /// test the locked joint constraint as well
644
645
646
  constraint = LockedJoint::create
    (ee2, ee2->configurationSpace ()->neutral ());

Le Quang Anh's avatar
Le Quang Anh committed
647
  std::cout << constraint->functionPtr()->name() << std::endl;
648
649
650
  joints = constraint->functionPtr()->dependsOnRelPoseBetween (device);
  BOOST_CHECK_EQUAL (joints.first->index(), ee2->parentJoint()->index());
  BOOST_CHECK_EQUAL (joints.second->index(), ee2->index());
Le Quang Anh's avatar
Le Quang Anh committed
651
652
  // constraint fully locks the joint
  jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
653
  BOOST_CHECK_EQUAL (jointsConstrained.second->index(), ee2->index());
Le Quang Anh's avatar
Le Quang Anh committed
654
}