joint.cc 21.7 KB
Newer Older
1
2
3
4
5
//
// Copyright (c) 2013, 2014 CNRS
// Author: Florent Lamiraux
//
//
6
7
8
9
10
11
12
13
14
15
16
17
18
19
// 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/>.

20
#include <hpp/fcl/math/vec_3f.h>
21
#include <hpp/util/debug.hh>
Florent Lamiraux's avatar
Florent Lamiraux committed
22
23
24
25
26
27
#include <hpp/model/body.hh>
#include <hpp/model/collision-object.hh>
#include <hpp/model/device.hh>
#include <hpp/model/fcl-to-eigen.hh>
#include <hpp/model/joint.hh>
#include <hpp/model/joint-configuration.hh>
28

29
#include <hpp/model/children-iterator.hh>
30

Florent Lamiraux's avatar
Florent Lamiraux committed
31
32
namespace hpp {
  namespace model {
Florent Lamiraux's avatar
Florent Lamiraux committed
33
34

    Joint::Joint (const Transform3f& initialPosition,
35
		  size_type configSize, size_type numberDof) :
Florent Lamiraux's avatar
Florent Lamiraux committed
36
37
      configuration_ (0x0), currentTransformation_ (initialPosition),
      positionInParentFrame_ (), T3f_ (), mass_ (0), massCom_ (),
38
      maximalDistanceToParent_ (0),
Florent Lamiraux's avatar
Florent Lamiraux committed
39
40
      configSize_ (configSize), numberDof_ (numberDof),
      initialPosition_ (initialPosition),
41
      robot_ (), body_ (0x0),
42
43
      name_ (), linkName_ (), children_ (), parent_ (0x0),
      rankInConfiguration_ (-1),
Florent Lamiraux's avatar
Florent Lamiraux committed
44
45
46
      jacobian_ (), rankInParent_ (0)
    {
      positionInParentFrame_.setIdentity ();
47
      linkInJointFrame_.setIdentity ();
Florent Lamiraux's avatar
Florent Lamiraux committed
48
49
      T3f_.setIdentity ();
      massCom_.setValue (0);
50
51
      neutralConfiguration_.resize (configSize);
      neutralConfiguration_.setZero ();
Florent Lamiraux's avatar
Florent Lamiraux committed
52
    }
53

Florent Lamiraux's avatar
Florent Lamiraux committed
54
55
56
    Joint::Joint (const Joint& joint) :
      configuration_ (joint.configuration_),
      positionInParentFrame_ (joint.positionInParentFrame_),
57
      linkInJointFrame_ (joint.linkInJointFrame_),
58
      maximalDistanceToParent_ (joint.maximalDistanceToParent_),
Florent Lamiraux's avatar
Florent Lamiraux committed
59
      configSize_ (joint.configSize_), numberDof_ (joint.numberDof_),
60
      robot_ (), body_ (joint.body_ ? joint.body_->clone (this) : 0x0),
61
      name_ (joint.name_), linkName_ (joint.linkName_),
Florent Lamiraux's avatar
Florent Lamiraux committed
62
63
64
      children_ (), parent_ (), rankInConfiguration_ (-1), rankInVelocity_ (-1),
      rankInParent_ (-1)
    {
65
      neutralConfiguration_ = joint.neutralConfiguration ();
Florent Lamiraux's avatar
Florent Lamiraux committed
66
67
    }

Florent Lamiraux's avatar
Florent Lamiraux committed
68
    Joint::~Joint ()
Florent Lamiraux's avatar
Florent Lamiraux committed
69
    {
Florent Lamiraux's avatar
Florent Lamiraux committed
70
71
      delete body_;
    }
Florent Lamiraux's avatar
Florent Lamiraux committed
72

Florent Lamiraux's avatar
Florent Lamiraux committed
73
74
75
    const Transform3f& Joint::initialPosition () const
    {
      return initialPosition_;
Florent Lamiraux's avatar
Florent Lamiraux committed
76
    }
77

Florent Lamiraux's avatar
Florent Lamiraux committed
78
    const Transform3f& Joint::currentTransformation () const
Florent Lamiraux's avatar
Florent Lamiraux committed
79
    {
Florent Lamiraux's avatar
Florent Lamiraux committed
80
81
82
      return currentTransformation_;
    }

83
84
    void Joint::addChildJoint (JointPtr_t joint,
			       bool computePositionInParent)
Florent Lamiraux's avatar
Florent Lamiraux committed
85
    {
86
87
      DevicePtr_t robot = robot_.lock ();
      if (!robot) {
Florent Lamiraux's avatar
Florent Lamiraux committed
88
89
	throw std::runtime_error
	  ("Cannot insert child joint to a joint not belonging to a device.");
Florent Lamiraux's avatar
Florent Lamiraux committed
90
      }
91
92
      joint->robot_ = robot;
      robot->registerJoint (joint);
Florent Lamiraux's avatar
Florent Lamiraux committed
93
94
95
96
97
98
      joint->rankInParent_ = children_.size ();
      children_.push_back (joint);
      joint->parent_ = this;
      // Mjoint/parent = Mparent^{-1} Mjoint
      fcl::Transform3f Mp = currentTransformation_;
      fcl::Transform3f Mj = joint->currentTransformation_;
99
      if (computePositionInParent) {
100
	joint->positionInParentFrame_ = Mp.inverse () * Mj;
101
	joint->computeMaximalDistanceToParent ();
102
      }
103
104
105
106
      // If child joint has been created by Joint::clone, bodies and list of
      // inner and outer objects have been copied without updating the number of
      // distance results.
      robot->updateDistances ();
107
      robot->computeMass ();
Florent Lamiraux's avatar
Florent Lamiraux committed
108
109
    }

110
111
112
113
114
115
116
    void Joint::positionInParentFrame (const Transform3f& p)
    {
      DevicePtr_t r = robot_.lock ();
      if (r) r->upToDate_ = false;
      positionInParentFrame_ = p;
    }

117
    void Joint::isBounded (size_type rank, bool bounded)
Florent Lamiraux's avatar
Florent Lamiraux committed
118
119
    {
      configuration_->isBounded (rank, bounded);
Florent Lamiraux's avatar
Florent Lamiraux committed
120
    }
121

122
    bool Joint::isBounded (size_type rank) const
Florent Lamiraux's avatar
Florent Lamiraux committed
123
    {
Florent Lamiraux's avatar
Florent Lamiraux committed
124
      return configuration_->isBounded (rank);
Florent Lamiraux's avatar
Florent Lamiraux committed
125
    }
126

127
    value_type Joint::lowerBound (size_type rank) const
Florent Lamiraux's avatar
Florent Lamiraux committed
128
    {
Florent Lamiraux's avatar
Florent Lamiraux committed
129
      return configuration_->lowerBound (rank);
Florent Lamiraux's avatar
Florent Lamiraux committed
130
    }
131

132
    value_type Joint::upperBound (size_type rank) const
Florent Lamiraux's avatar
Florent Lamiraux committed
133
    {
Florent Lamiraux's avatar
Florent Lamiraux committed
134
      return configuration_->upperBound (rank);
Florent Lamiraux's avatar
Florent Lamiraux committed
135
    }
136

137
    void Joint::lowerBound (size_type rank, value_type lower)
Florent Lamiraux's avatar
Florent Lamiraux committed
138
139
    {
      configuration_->lowerBound (rank, lower);
140
      computeMaximalDistanceToParent ();
Florent Lamiraux's avatar
Florent Lamiraux committed
141
    }
142

143
    void Joint::upperBound (size_type rank, value_type upper)
Florent Lamiraux's avatar
Florent Lamiraux committed
144
    {
Florent Lamiraux's avatar
Florent Lamiraux committed
145
      configuration_->upperBound (rank, upper);
146
      computeMaximalDistanceToParent ();
Florent Lamiraux's avatar
Florent Lamiraux committed
147
    }
148

149
    BodyPtr_t Joint::linkedBody () const
Florent Lamiraux's avatar
Florent Lamiraux committed
150
    {
Florent Lamiraux's avatar
Florent Lamiraux committed
151
152
153
      return body_;
    }

154
    void Joint::setLinkedBody (const BodyPtr_t& body) {
Florent Lamiraux's avatar
Florent Lamiraux committed
155
156
      body_ = body;
      body->joint (this);
157
158
159
      DevicePtr_t robot = robot_.lock ();
      if (robot) {
	robot->computeMass ();
Florent Lamiraux's avatar
Florent Lamiraux committed
160
      }
Florent Lamiraux's avatar
Florent Lamiraux committed
161
    }
162

Florent Lamiraux's avatar
Florent Lamiraux committed
163
    void Joint::recursiveComputePosition (ConfigurationIn_t configuration,
164
					  const Transform3f& parentPosition)
165
      const
Florent Lamiraux's avatar
Florent Lamiraux committed
166
    {
167
      computePosition (configuration, parentPosition, currentTransformation_);
168
      for (std::vector <JointPtr_t>::const_iterator itJoint =
Joseph Mirabel's avatar
Joseph Mirabel committed
169
	     children_.begin (); itJoint != children_.end (); ++itJoint) {
Florent Lamiraux's avatar
Florent Lamiraux committed
170
171
	(*itJoint)->recursiveComputePosition
	  (configuration, currentTransformation_);
Florent Lamiraux's avatar
Florent Lamiraux committed
172
173
      }
    }
174

Florent Lamiraux's avatar
Florent Lamiraux committed
175
    void Joint::computeJacobian ()
Florent Lamiraux's avatar
Florent Lamiraux committed
176
    {
Florent Lamiraux's avatar
Florent Lamiraux committed
177
178
179
180
      for (ChildrenIterator it1 (this); !it1.end (); ++it1) {
	for (ChildrenIterator it2 (*it1); !it2.end (); ++it2) {
	  (*it1)->writeSubJacobian (*it2);
	}
Florent Lamiraux's avatar
Florent Lamiraux committed
181
182
      }
    }
183

184
    value_type Joint::computeMass ()
Florent Lamiraux's avatar
Florent Lamiraux committed
185
    {
Florent Lamiraux's avatar
Florent Lamiraux committed
186
187
188
      mass_ = 0;
      if (body_) {
	mass_ = body_->mass ();
Florent Lamiraux's avatar
Florent Lamiraux committed
189
      }
Florent Lamiraux's avatar
Florent Lamiraux committed
190
      for (std::vector <JointPtr_t>::iterator it =  children_.begin ();
Joseph Mirabel's avatar
Joseph Mirabel committed
191
	   it != children_.end (); ++it) {
Florent Lamiraux's avatar
Florent Lamiraux committed
192
	mass_ += (*it)->computeMass ();
193
      }
Florent Lamiraux's avatar
Florent Lamiraux committed
194
      return mass_;
Florent Lamiraux's avatar
Florent Lamiraux committed
195
    }
196

Florent Lamiraux's avatar
Florent Lamiraux committed
197
    void Joint::computeMassTimesCenterOfMass ()
Florent Lamiraux's avatar
Florent Lamiraux committed
198
    {
Florent Lamiraux's avatar
Florent Lamiraux committed
199
200
201
202
203
204
      massCom_.setValue (0);
      if (body_) {
	massCom_ = currentTransformation_.transform
	  (body_->localCenterOfMass ()) * body_->mass ();
      }
      for (std::vector <JointPtr_t>::iterator it =  children_.begin ();
Joseph Mirabel's avatar
Joseph Mirabel committed
205
	   it != children_.end (); ++it) {
Florent Lamiraux's avatar
Florent Lamiraux committed
206
207
208
	(*it)->computeMassTimesCenterOfMass ();
	massCom_ += (*it)->massCom_;
      }
Florent Lamiraux's avatar
Florent Lamiraux committed
209
    }
210

Florent Lamiraux's avatar
Florent Lamiraux committed
211
212
213
214
215
    JointAnchor::JointAnchor (const Transform3f& initialPosition) :
      Joint (initialPosition, 0, 0)
    {
      configuration_ = new AnchorJointConfig;
    }
216

Florent Lamiraux's avatar
Florent Lamiraux committed
217
218
219
220
221
222
223
224
225
226
    JointAnchor::JointAnchor (const JointAnchor& joint) :
      Joint (joint)
    {
    }

    JointPtr_t JointAnchor::clone () const
    {
      return new JointAnchor (*this);
    }

Florent Lamiraux's avatar
Florent Lamiraux committed
227
    JointAnchor::~JointAnchor ()
Florent Lamiraux's avatar
Florent Lamiraux committed
228
    {
Florent Lamiraux's avatar
Florent Lamiraux committed
229
      delete configuration_;
230
231
    }

232
233
234
235
236
237
    void JointAnchor::computeMaximalDistanceToParent ()
    {
      maximalDistanceToParent_ =
	positionInParentFrame ().getTranslation ().length ();
    }

238
239
    void JointAnchor::computePosition (ConfigurationIn_t,
				       const Transform3f& parentPosition,
240
				       Transform3f& position) const
Florent Lamiraux's avatar
Florent Lamiraux committed
241
    {
242
      position = parentPosition * positionInParentFrame_;
Florent Lamiraux's avatar
Florent Lamiraux committed
243
    }
244

Florent Lamiraux's avatar
Florent Lamiraux committed
245
    void JointAnchor::writeSubJacobian (const JointPtr_t&)
Florent Lamiraux's avatar
Florent Lamiraux committed
246
    {
247
    }
248

Florent Lamiraux's avatar
Florent Lamiraux committed
249
    void JointAnchor::writeComSubjacobian (ComJacobian_t&,
250
					   const value_type&)
Florent Lamiraux's avatar
Florent Lamiraux committed
251
252
    {
    }
253

Florent Lamiraux's avatar
Florent Lamiraux committed
254
255
    JointSO3::JointSO3 (const Transform3f& initialPosition) :
      Joint (initialPosition, 4, 3)
256
    {
Florent Lamiraux's avatar
Florent Lamiraux committed
257
      configuration_ = new SO3JointConfig;
258
      neutralConfiguration_ [0] = 1;
259
260
    }

Florent Lamiraux's avatar
Florent Lamiraux committed
261
262
263
264
265
266
267
268
269
270
    JointSO3::JointSO3 (const JointSO3& joint) :
      Joint (joint)
    {
    }

    JointPtr_t JointSO3::clone () const
    {
      return new JointSO3 (*this);
    }

Florent Lamiraux's avatar
Florent Lamiraux committed
271
272
273
274
    JointSO3::~JointSO3 ()
    {
      delete configuration_;
    }
275

276
277
278
279
280
281
    void JointSO3::computeMaximalDistanceToParent ()
    {
      maximalDistanceToParent_ =
	positionInParentFrame ().getTranslation ().length ();
    }

282
283
    void JointSO3::computePosition (ConfigurationIn_t configuration,
				    const Transform3f& parentPosition,
284
				    Transform3f& position) const
Florent Lamiraux's avatar
Florent Lamiraux committed
285
286
287
288
289
290
    {
      fcl::Quaternion3f p (configuration [rankInConfiguration ()],
			   configuration [rankInConfiguration () + 1],
			   configuration [rankInConfiguration () + 2],
			   configuration [rankInConfiguration () + 3]);
      T3f_.setQuatRotation (p);
291
      position = parentPosition * positionInParentFrame_ * T3f_;
Florent Lamiraux's avatar
Florent Lamiraux committed
292
293
294
295
296
297
298
299
    }
    static void cross (const fcl::Vec3f& x, JointJacobian_t& J, size_type row,
		       size_type col)
    {
      J (row + 0, col + 1) = -x [2]; J (row + 1, col + 0) = x [2];
      J (row + 0, col + 2) = x [1]; J (row + 2, col + 0) = -x [1];
      J (row + 1, col + 2) = -x [0]; J (row + 2, col + 1) = x [0];
    }
300

Florent Lamiraux's avatar
Florent Lamiraux committed
301
302
    static void cross (const fcl::Vec3f& x, ComJacobian_t& J, size_type row,
		       size_type col)
303
    {
Florent Lamiraux's avatar
Florent Lamiraux committed
304
305
306
307
      J (row + 0, col + 1) = -x [2]; J (row + 1, col + 0) = x [2];
      J (row + 0, col + 2) = x [1]; J (row + 2, col + 0) = -x [1];
      J (row + 1, col + 2) = -x [0]; J (row + 2, col + 1) = x [0];
    }
308

Florent Lamiraux's avatar
Florent Lamiraux committed
309
310
    void JointSO3::writeSubJacobian (const JointPtr_t& child)
    {
311
      size_type col = rankInVelocity ();
Florent Lamiraux's avatar
Florent Lamiraux committed
312
313
314
315
316
317
318
319
320
      // Set diagonal terms to 1
      child->jacobian () (3,col) = child->jacobian () (4,col+1) =
	child->jacobian () (5,col+2) = 1;
      cross (currentTransformation_.getTranslation () -
	     child->currentTransformation ().getTranslation (),
	     child->jacobian (), 0, col);
    }

    void JointSO3::writeComSubjacobian (ComJacobian_t& jacobian,
321
					const value_type& totalMass)
Florent Lamiraux's avatar
Florent Lamiraux committed
322
    {
323
324
325
326
327
328
      if (mass_ > 0) {
	const fcl::Vec3f& center (currentTransformation_.getTranslation ());
	com_ = massCom_ * (1/mass_);
	size_type col = rankInVelocity ();
	cross ((mass_/totalMass)*(center-com_), jacobian, (size_type) 0, col);
      }
329
330
    }

331
332
333
    JointRotation::JointRotation (const Transform3f& initialPosition,
				  size_type configSize, size_type numberDof) :
      Joint (initialPosition, configSize, numberDof), R_ ()
Florent Lamiraux's avatar
Florent Lamiraux committed
334
335
336
337
    {
      R_.setIdentity ();
    }

Florent Lamiraux's avatar
Florent Lamiraux committed
338
339
340
341
342
343
    JointRotation::JointRotation (const JointRotation& joint) :
      Joint (joint), R_ ()
    {
      R_.setIdentity ();
    }

Florent Lamiraux's avatar
Florent Lamiraux committed
344
345
346
347
348
    JointRotation::~JointRotation ()
    {
      delete configuration_;
    }

349
350
351
352
353
354
    void JointRotation::computeMaximalDistanceToParent ()
    {
      maximalDistanceToParent_ =
	positionInParentFrame ().getTranslation ().length ();
    }

Florent Lamiraux's avatar
Florent Lamiraux committed
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
    void JointRotation::writeSubJacobian (const JointPtr_t& child)
    {
      size_type col = rankInVelocity ();
      // Get rotation axis
      axis_ = currentTransformation_.getRotation ().getColumn (0);
      O2O1_ = currentTransformation_.getTranslation () -
	child->currentTransformation ().getTranslation ();
      cross_ = O2O1_.cross (axis_);
      child->jacobian () (0, col) = cross_ [0];
      child->jacobian () (1, col) = cross_ [1];
      child->jacobian () (2, col) = cross_ [2];
      child->jacobian () (3, col) = axis_ [0];
      child->jacobian () (4, col) = axis_ [1];
      child->jacobian () (5, col) = axis_ [2];
    }

    void JointRotation::writeComSubjacobian (ComJacobian_t& jacobian,
372
					     const value_type& totalMass)
Florent Lamiraux's avatar
Florent Lamiraux committed
373
    {
374
375
376
377
378
379
380
381
382
383
384
      if (mass_ > 0) {
	size_type col = rankInVelocity ();
	axis_ = currentTransformation_.getRotation ().getColumn (0);
	com_ = massCom_ * (1/mass_);
	const fcl::Vec3f& center (currentTransformation_.getTranslation ());
	O2O1_ = center - com_;
	cross_ = (mass_/totalMass) * O2O1_.cross (axis_);
	jacobian (0, col) = cross_ [0];
	jacobian (1, col) = cross_ [1];
	jacobian (2, col) = cross_ [2];
      }
Florent Lamiraux's avatar
Florent Lamiraux committed
385
386
    }

387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
    namespace jointRotation {

      UnBounded::UnBounded (const Transform3f& initialPosition) :
	JointRotation (initialPosition, 2, 1)
      {
	configuration_ = new rotationJointConfig::UnBounded;
	neutralConfiguration_ [0] = 1;
      }

      UnBounded::UnBounded (const UnBounded& joint) : JointRotation (joint)
      {
      }

      JointPtr_t UnBounded::clone () const
      {
	return new UnBounded (*this);
      }

      void UnBounded::computePosition (ConfigurationIn_t configuration,
				       const Transform3f& parentPosition,
				       Transform3f& position) const
      {
	cosAngle_ = configuration [rankInConfiguration ()];
	sinAngle_ = configuration [rankInConfiguration () + 1];
	R_ (1,1) = cosAngle_; R_ (1,2) = -sinAngle_;
	R_ (2,1) = sinAngle_; R_ (2,2) = cosAngle_;
	T3f_.setRotation (R_);
	position = parentPosition * positionInParentFrame_ * T3f_;
      }

      Bounded::Bounded (const Transform3f& initialPosition) :
	JointRotation (initialPosition, 1, 1)
      {
	configuration_ = new rotationJointConfig::Bounded;
      }

      Bounded::Bounded (const Bounded& joint) : JointRotation (joint)
      {
      }

      JointPtr_t Bounded::clone () const
      {
	return new Bounded (*this);
      }

      void Bounded::computePosition (ConfigurationIn_t configuration,
				     const Transform3f& parentPosition,
				     Transform3f& position) const
      {
	angle_ = configuration [rankInConfiguration ()];
	R_ (1,1) = cos (angle_); R_ (1,2) = -sin (angle_);
	R_ (2,1) = sin (angle_); R_ (2,2) = cos (angle_);
	T3f_.setRotation (R_);
	position = parentPosition * positionInParentFrame_ * T3f_;
      }
    } // namespace jointRotation

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
    template <size_type dimension>
    JointTranslation <dimension>::JointTranslation
    (const Transform3f& initialPosition) : Joint (initialPosition, dimension,
						  dimension), t_ ()
    {
      if (dimension > 3 || dimension ==0) {
	throw std::runtime_error
	  ("Dimension of translation should be between 1 and 3.");
      }
      configuration_ = new TranslationJointConfig <dimension>;
      t_.setValue (0);
    }

    template <size_type dimension>
    JointTranslation <dimension>::JointTranslation
    (const JointTranslation <dimension>& joint) : Joint (joint), t_ ()
    {
      t_.setValue (0);
    }

    template <size_type dimension>
    JointPtr_t JointTranslation <dimension>::clone () const
    {
      return new JointTranslation <dimension> (*this);
    }

    template <size_type dimension>
    JointTranslation <dimension>::~JointTranslation ()
    {
      delete configuration_;
    }

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
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
    template <size_type dimension>
    void JointTranslation <dimension>::computeMaximalDistanceToParent ()
    {
      bool bounded = true;
      for (size_type i=0; i<dimension; ++i) {
	if (!isBounded (i)) bounded = false;
      }
      if (bounded) {
	const Transform3f& pos = positionInParentFrame ();
	const fcl::Vec3f& T = pos.getTranslation ();
	maximalDistanceToParent_ = 0;
	switch (dimension) {
	case 1:
	  {
	    fcl::Vec3f u0 = pos.getRotation ().getColumn (0);
	    value_type d = (T + lowerBound (0)*u0).length ();
	    if (d > maximalDistanceToParent_)
	      maximalDistanceToParent_ = d;
	    d = (T + upperBound (0)*u0).length ();
	    if (d > maximalDistanceToParent_)
	      maximalDistanceToParent_ = d;
	  }
	  break;
	case 2:
	  {
	    fcl::Vec3f u0 = pos.getRotation ().getColumn (0);
	    fcl::Vec3f u1 = pos.getRotation ().getColumn (1);
	    value_type d = (T + lowerBound (0)*u0 +
			    lowerBound (1)*u1).length ();
	    if (d > maximalDistanceToParent_)
	      maximalDistanceToParent_ = d;
	    d = (T + upperBound (0)*u0 + lowerBound (1)*u1).length ();
	    if (d > maximalDistanceToParent_)
	      maximalDistanceToParent_ = d;
	    d = (T + lowerBound (0)*u0 + upperBound (1)*u1).length ();
	    if (d > maximalDistanceToParent_)
	      maximalDistanceToParent_ = d;
	    d = (T + upperBound (0)*u0 + upperBound (1)*u1).length ();
	    if (d > maximalDistanceToParent_)
	      maximalDistanceToParent_ = d;
	  }
	  break;
	case 3:
	  {
	    fcl::Vec3f u0 = pos.getRotation ().getColumn (0);
	    fcl::Vec3f u1 = pos.getRotation ().getColumn (1);
	    fcl::Vec3f u2 = pos.getRotation ().getColumn (2);
	    value_type d = (T + lowerBound (0)*u0 +
			    lowerBound (1)*u1 +
			    lowerBound (2)*u2).length ();
	    if (d > maximalDistanceToParent_)
	      maximalDistanceToParent_ = d;
	    d = (T + upperBound (0)*u0 + lowerBound (1)*u1 +
		 lowerBound (2)*u2).length ();
	    if (d > maximalDistanceToParent_)
	      maximalDistanceToParent_ = d;
	    d = (T + lowerBound (0)*u0 + upperBound (1)*u1 +
		 lowerBound (2)*u2).length ();
	    if (d > maximalDistanceToParent_)
	      maximalDistanceToParent_ = d;
	    d = (T + upperBound (0)*u0 + upperBound (1)*u1 +
		 lowerBound (2)*u2).length ();
	    if (d > maximalDistanceToParent_)
	      maximalDistanceToParent_ = d;
	    d = (T + lowerBound (0)*u0 + lowerBound (1)*u1 +
		 upperBound (2)*u2).length ();
	    if (d > maximalDistanceToParent_)
	      maximalDistanceToParent_ = d;
	    d = (T + upperBound (0)*u0 + lowerBound (1)*u1 +
		 upperBound (2)*u2).length ();
	    if (d > maximalDistanceToParent_)
	      maximalDistanceToParent_ = d;
	    d = (T + lowerBound (0)*u0 + upperBound (1)*u1 +
		 upperBound (2)*u2).length ();
	    if (d > maximalDistanceToParent_)
	      maximalDistanceToParent_ = d;
	    d = (T + upperBound (0)*u0 + upperBound (1)*u1 +
		 upperBound (2)*u2).length ();
	    if (d > maximalDistanceToParent_)
	      maximalDistanceToParent_ = d;
	  }
	  break;
	default:
	  assert (false && "dimension should be included between 1 and 3.");
	}
      } else {
	maximalDistanceToParent_ =
	  std::numeric_limits <value_type>::infinity ();
      }
    }

567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
    template <size_type dimension>
    void JointTranslation <dimension>::computePosition
    (ConfigurationIn_t configuration, const Transform3f& parentPosition,
     Transform3f& position) const
    {
      t_ [0] = configuration [rankInConfiguration ()];
      if (dimension >= 2) {
	t_ [1] = configuration [rankInConfiguration () + 1];
      }
      if (dimension >= 3) {
	t_ [2] = configuration [rankInConfiguration () + 2];
      }
      T3f_.setTranslation (t_);
      position = parentPosition * positionInParentFrame_ * T3f_;
    }
Florent Lamiraux's avatar
Florent Lamiraux committed
582

583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
    template <size_type dimension>
    void JointTranslation <dimension>::writeSubJacobian
    (const JointPtr_t& child)
    {
      size_type col = rankInVelocity ();
      // Get translation axis
      for (unsigned int i=0; i<dimension; ++i) {
	axis_[i] = currentTransformation_.getRotation ().getColumn (i);
	child->jacobian () (0, col+i) = axis_ [i][0];
	child->jacobian () (1, col+i) = axis_ [i][1];
	child->jacobian () (2, col+i) = axis_ [i][2];
      }
    }

    template <size_type dimension>
    void JointTranslation <dimension>::writeComSubjacobian
599
    (ComJacobian_t& jacobian, const value_type& totalMass)
600
601
602
603
604
605
606
607
608
609
610
611
    {
      if (mass_ > 0) {
	size_type col = rankInVelocity ();
	// Get translation axis
	for (unsigned int i=0; i<dimension; ++i) {
	  axis_ [i] = currentTransformation_.getRotation ().getColumn (i);
	  jacobian (0, col+i) = (mass_/totalMass) * axis_ [i][0];
	  jacobian (1, col+i) = (mass_/totalMass) * axis_ [i][1];
	  jacobian (2, col+i) = (mass_/totalMass) * axis_ [i][2];
	}
      }
    }
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651

    std::ostream& displayTransform3f (std::ostream& os,
				      const fcl::Transform3f trans)
    {
      const fcl::Matrix3f& R (trans.getRotation ());
      const fcl::Vec3f& t (trans.getTranslation ());
      const fcl::Quaternion3f& q (trans.getQuatRotation ());
      os << "rotation matrix: " << R << "\\n";
      os << "rotation quaternion: " << "(" << q.getW () << ", "
	 << q.getX () << ", " << q.getY () << ", " << q.getZ () << ")"
	 << "\\n";
      os << "translation: " << t;
      return os;
    }

    std::ostream& Joint::display (std::ostream& os) const
    {
      os << "\"" << name () << "\"" << "[shape=box label=\"" << name ()
	 << "\\n";
      if (configSize () != 0)
	os << "Rank in configuration: " << rankInConfiguration() << "\\n";
      else
	os << "Anchor joint\\n";
      os << "Current transformation: ";
      displayTransform3f (os, currentTransformation());
      os << "\\n";
      hpp::model::BodyPtr_t body = linkedBody();
      if (body) {
	const matrix3_t& I = body->inertiaMatrix();
	os << "Attached body: " << body->name () << "\\n";
	os << "Mass of the attached body: " << body->mass() << "\\n";
	os << "Local center of mass:" << body->localCenterOfMass() << "\\n";
	os << "Inertia matrix:" << "\\n";
	os << I (0,0) << "\t" << I (0,1) << "\t" << I (0,2) << "\\n"
	   << I (1,0) << "\t" << I (1,1) << "\t" << I (1,2) << "\\n"
	   << I (2,0) << "\t" << I (2,1) << "\t" << I (2,2) << "\\n";
	os << "geometric objects" << "\\n";
	const hpp::model::ObjectVector_t& colObjects =
	  body->innerObjects (hpp::model::COLLISION);
	for (hpp::model::ObjectVector_t::const_iterator it =
Joseph Mirabel's avatar
Joseph Mirabel committed
652
	       colObjects.begin (); it != colObjects.end (); ++it) {
653
	  os << "name: " << (*it)->name () << "\\n";
654
655
656
	  os << "position in joint:" << "\\n";
	  const fcl::Transform3f& local ((*it)->positionInJointFrame ());
	  displayTransform3f (os, local); os << "\\n";
657
	  os << "position :" << "\\n";
658
659
	  const fcl::Transform3f& global ((*it)->fcl ()->getTransform ());
	  displayTransform3f (os, global);
660
661
662
663
664
665
666
667
668
669
670
671
	}
      } else {
	os << "No body";
      }
      os << "\"]" << std::endl;
      for (unsigned int iChild=0; iChild < numberChildJoints ();
	   iChild++) {
	hpp::model::JointPtr_t child = childJoint (iChild);
	os << *(child) << std::endl;
      }
      // write edges to children joints
      for (hpp::model::JointVector_t::const_iterator it =
Joseph Mirabel's avatar
Joseph Mirabel committed
672
	     children_.begin (); it != children_.end (); ++it) {
673
674
675
676
677
	os << "\"" << name () << "\"->\"" << (*it)->name () << "\""
	   << std::endl;
      }
      return os;
    }
678
679
680
    template class JointTranslation <1>;
    template class JointTranslation <2>;
    template class JointTranslation <3>;
681
682
683
684
685

    std::ostream& operator<< (std::ostream& os, const hpp::model::Joint& joint)
    {
      return joint.display (os);
    }
Florent Lamiraux's avatar
Florent Lamiraux committed
686
687
  } // namespace model
} // namespace hpp
688

689
690
691
692
693
694
695
696
697
698
699
700
701
namespace fcl {
  std::ostream& operator<< (std::ostream& os , const fcl::Transform3f& trans)
  {
    const fcl::Matrix3f& R (trans.getRotation ());
    const fcl::Vec3f& t (trans.getTranslation ());
    const fcl::Quaternion3f& q (trans.getQuatRotation ());
    os << "rotation matrix: " << R << std::endl;
    os << "rotation quaternion: " << "(" << q.getW () << ", "
       << q.getX () << ", " << q.getY () << ", " << q.getZ () << ")"
       << std::endl;
    os << "translation: " << t << std::endl;
    return os;
  }
702
}