test-configuration.cc 8.4 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
///
/// Copyright (c) 2011 CNRS
/// Author: Florent Lamiraux
///
///
// This file is part of hpp-model
// hpp-model 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-model 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-model  If not, see
// <http://www.gnu.org/licenses/>.

// This test
//   - builds a robot with various types of joints,
//   - randomly samples pairs of configurations,
//   - test consistency between hpp::model::difference and
//     hpp::model::integrate functions.

#include <sstream>

#define BOOST_TEST_MODULE TEST_CONFIGURATION
#include <boost/test/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>
using boost::test_tools::output_test_stream;

33
34
#include <Eigen/Geometry>

35
36
37
#include <hpp/util/debug.hh>
#include <hpp/model/configuration.hh>
#include <hpp/model/object-factory.hh>
38
#include <hpp/model/fcl-to-eigen.hh>
39
40

using hpp::model::vector_t;
41
using hpp::model::vectorIn_t;
42
43
44
45
46
47
48
49
50
51
52
53
using hpp::model::Configuration_t;
using hpp::model::JointPtr_t;
using hpp::model::ObjectFactory;
using hpp::model::Transform3f;
using fcl::Quaternion3f;
using hpp::model::Device;
using hpp::model::DevicePtr_t;
using hpp::model::JointVector_t;
using hpp::model::ConfigurationPtr_t;
using hpp::model::size_type;
using hpp::model::value_type;

54
55
56
typedef Eigen::AngleAxis <value_type> AngleAxis_t;
typedef Eigen::Quaternion <value_type> Quaternion_t;

57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
// Create a robot with various types of joints
DevicePtr_t createRobot ()
{
  DevicePtr_t robot = Device::create ("robot");
  Transform3f position; position.setIdentity ();
  ObjectFactory factory;

  // planar root joint
  JointPtr_t root = factory.createJointTranslation2 (position);
  robot->rootJoint (root);
  root->isBounded (0, true);
  root->lowerBound (0, -1);
  root->upperBound (0, 1);
  root->isBounded (1, true);
  root->lowerBound (1, -1);
  root->upperBound (1, 1);
  // Rotation around z
  position.setQuatRotation (Quaternion3f (sqrt (2)/2, 0, -sqrt (2)/2, 0));
  JointPtr_t joint = factory.createUnBoundedJointRotation (position);
  root->addChildJoint (joint);
  position.setIdentity ();
  // SO3 joint
  position.setIdentity ();
  JointPtr_t j1 = factory.createJointSO3 (position);
81
  j1->name ("so3");
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
  joint->addChildJoint (j1);

  return robot;
}

void shootRandomConfig (const DevicePtr_t& robot, Configuration_t& config)
{
  JointVector_t jv = robot->getJointVector ();
  for (JointVector_t::const_iterator itJoint = jv.begin ();
       itJoint != jv.end (); itJoint++) {
    std::size_t rank = (*itJoint)->rankInConfiguration ();
    (*itJoint)->configuration ()->uniformlySample (rank, config);
  }
  // Shoot extra configuration variables
  size_type extraDim = robot->extraConfigSpace ().dimension ();
  size_type offset = robot->configSize () - extraDim;
  for (size_type i=0; i<extraDim; ++i) {
    value_type lower = robot->extraConfigSpace ().lower (i);
    value_type upper = robot->extraConfigSpace ().upper (i);
    value_type range = upper - lower;
    if ((range < 0) ||
	(range == std::numeric_limits<double>::infinity())) {
      std::ostringstream oss
	("Cannot uniformy sample extra config variable ");
      oss << i << ". min = " << ", max = " << upper << std::endl;
      throw std::runtime_error (oss.str ());
    }
    config [offset + i] = (upper - lower) * rand ()/RAND_MAX;
  }
}

113
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
147
148
149
150
151
152
153
154
155
156
157
158
vector_t slerp (vectorIn_t v0, vectorIn_t v1, const value_type t) {
  Quaternion_t q0(v0[0], v0[1], v0[2], v0[3]);
  Quaternion_t q1(v1[0], v1[1], v1[2], v1[3]);
  Quaternion_t q = q0.slerp (t, q1);
  vector_t res(4);
  res[0] = q.w(); res[1] = q.x(); res[2] = q.y(); res[3] = q.z();
  return res;
}

AngleAxis_t aa (vectorIn_t v0, vectorIn_t v1) {
  Quaternion_t q0(v0[0], v0[1], v0[2], v0[3]);
  Quaternion_t q1(v1[0], v1[1], v1[2], v1[3]);
  return AngleAxis_t (q0 * q1.conjugate ());
}

vector_t interpolation (DevicePtr_t robot, vectorIn_t q0, vectorIn_t q1, int n)
{
  const size_type rso3 = 4;
  bool print = false;

  vector_t q2 (q0);
  value_type u = 0;
  value_type step = (value_type)1 / (value_type)(n + 2);
  vector_t angle1 (n+2), angle2(n+2);

  if (print) std::cout << "HPP  : ";
  for (int i = 0; i < n + 2; ++i) {
    u = 0 + i * step; 
    hpp::model::interpolate (robot, q0, q1, u, q2);
    AngleAxis_t aa1 (aa (q0.segment<4>(rso3),q2.segment<4>(rso3)));
    angle1[i] = aa1.angle ();
    if (print) std::cout << aa1.angle () << ", ";
  }
  if (print) std::cout << "\n";
  if (print) std::cout << "Eigen: ";
  for (int i = 0; i < n + 2; ++i) {
    u = 0 + i * step; 
    vector_t eigen_slerp = slerp (q0.segment<4>(rso3), q1.segment<4>(rso3), u);
    AngleAxis_t aa2 (aa (q0.segment<4>(rso3),eigen_slerp));
    angle2[i] = aa2.angle ();
    if (print) std::cout << aa2.angle () << ", ";
  }
  if (print) std::cout << "\n";
  return angle1 - angle2;
}

159
160
161
162
163
164
165
BOOST_AUTO_TEST_CASE(difference_and_integrate)
{
  DevicePtr_t robot = createRobot ();
  Configuration_t q0; q0.resize (robot->configSize ());
  Configuration_t q1; q1.resize (robot->configSize ());
  Configuration_t q2; q2.resize (robot->configSize ());
  vector_t q1_minus_q0; q1_minus_q0.resize (robot->numberDof ());
166
  const value_type eps_dist = robot->numberDof() * sqrt(Eigen::NumTraits<value_type>::epsilon());
167
168
169
  for (size_type i=0; i<10000; ++i) {
    shootRandomConfig (robot, q0);
    shootRandomConfig (robot, q1);
170

171
172
    hpp::model::difference (robot, q1, q0, q1_minus_q0);
    hpp::model::integrate (robot, q0, q1_minus_q0, q2);
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191

    // Check that the distance is the norm of the difference
    value_type distance = hpp::model::distance (robot, q0, q1);
    BOOST_CHECK_MESSAGE (distance - q1_minus_q0.norm () < Eigen::NumTraits<value_type>::dummy_precision(),
        "\nThe distance is not the norm of the difference\n" <<
        "q0=" << q0.transpose () << "\n" <<
        "q1=" << q1.transpose () << "\n" <<
        "distance=" << distance  << "\n" <<
        "(q1 - q0).norm () = " << q1_minus_q0.norm ()
        );

    // Check that distance (q0 + (q1 - q0), q1) is zero
    distance = hpp::model::distance (robot, q1, q2);
    BOOST_CHECK_MESSAGE (distance < eps_dist,
        "\n(q0 + (q1 - q0)) is not equivalent to q1\n" <<
        "q1=" << q1.transpose () << "\n" <<
        "q2=" << q2.transpose () << "\n" <<
        "distance=" << distance
        );
192
193
  }
}
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234

BOOST_AUTO_TEST_CASE(interpolate)
{
  DevicePtr_t robot = createRobot ();
  Configuration_t q0; q0.resize (robot->configSize ());
  Configuration_t q1; q1.resize (robot->configSize ());
  Configuration_t q2; q2.resize (robot->configSize ());
  vector_t q1_minus_q0; q1_minus_q0.resize (robot->numberDof ());
  const value_type eps_dist = robot->numberDof() * sqrt(Eigen::NumTraits<value_type>::epsilon());
  value_type distance;
  for (size_type i=0; i<10000; ++i) {
    shootRandomConfig (robot, q0);
    shootRandomConfig (robot, q1);

    hpp::model::interpolate (robot, q0, q1, 0, q2);
    distance = hpp::model::distance (robot, q0, q2);
    BOOST_CHECK_MESSAGE (distance < eps_dist,
        "\n(q0 + 0 * (q1 - q0)) is not equivalent to q0\n" <<
        "q0=" << q0.transpose () << "\n" <<
        "q1=" << q1.transpose () << "\n" <<
        "q2=" << q2.transpose () << "\n" <<
        "distance=" << distance
        );

    const size_type rso3 = 4;
    vector_t errors = interpolation (robot, q0, q1, 4);
    BOOST_CHECK_MESSAGE (errors.isZero (),
        "The interpolation computed by HPP does not match the Eigen SLERP"
        );

    hpp::model::interpolate (robot, q0, q1, 1, q2);
    distance = hpp::model::distance (robot, q1, q2);
    BOOST_CHECK_MESSAGE (distance < eps_dist,
        "\n(q0 + 1 * (q1 - q0)) is not equivalent to q1\n" <<
        "q0=" << q0.transpose () << "\n" <<
        "q1=" << q1.transpose () << "\n" <<
        "q2=" << q2.transpose () << "\n" <<
        "distance=" << distance
        );
  }
}
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252

BOOST_AUTO_TEST_CASE(toEigenFunction)
{
  // Compilation check only.

  using namespace hpp::model;
  vector3_t fcl_v;
  matrix3_t fcl_m;
  fcl_v.setValue (1);
  fcl_m.setValue (3);

  vector_t eigen_v(3);
  matrix_t eigen_m(3,3);

  toEigen (fcl_v, eigen_v);
  toEigen (fcl_m, eigen_m);
  toEigen (fcl_v, eigen_m.col(0));
}