joint.hh 13.4 KB
Newer Older
1
//
Nicolas Mansard's avatar
Nicolas Mansard committed
2
3
// Copyright (c) 2016 CNRS
// Author: NMansard from Florent Lamiraux
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
//
//
// This file is part of hpp-pinocchio
// hpp-pinocchio 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-pinocchio 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-pinocchio  If not, see
// <http://www.gnu.org/licenses/>.

#ifndef HPP_PINOCCHIO_JOINT_HH
# define HPP_PINOCCHIO_JOINT_HH

# include <cstddef>
# include <hpp/fcl/math/transform.h>
# include <hpp/pinocchio/config.hh>
# include <hpp/pinocchio/fwd.hh>
27
# include <hpp/pinocchio/fake-container.hh>
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58

namespace hpp {
  namespace pinocchio {
    /// Robot joint
    ///
    /// A joint maps an input vector to a transformation of SE(3) from the
    /// parent frame to the joint frame.
    ///
    /// The input vector is provided through the configuration vector of the
    /// robot the joint belongs to. The joint input vector is composed of the
    /// components of the robot configuration starting at
    /// Joint::rankInConfiguration.
    ///
    /// The joint input vector represents a element of a Lie group, either
    /// \li a vector space for JointTranslation, and bounded JointRotation,
    /// \li the unit circle for non-bounded JointRotation joints,
    /// \li an element of SO(3) for JointSO3, represented by a unit quaternion.
    ///
    /// Operations specific to joints (uniform sampling of input space, straight
    /// interpolation, distance, ...) are performed by a JointConfiguration
    /// instance that has the same class hierarchy as Joint.
    class HPP_PINOCCHIO_DLLAPI Joint {
    public:
      typedef se3::Index Index;

      /// \name Construction and copy and destruction
      /// \{

      /// Constructor
      /// \param device pointer on the device the joint is belonging to.
      /// \param indexInJointList index of the joint, i.e. joint = device.model.joints[index]
59
      Joint (DevicePtr_t device, Index indexInJointList );
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
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
113

      //DEPREC /// Constructor
      //DEPREC /// \param initialPosition position of the joint before being inserted
      //DEPREC ///        in a kinematic chain,
      //DEPREC /// \param configSize dimension of the configuration vector,
      //DEPREC /// \param numberDof dimension of the velocity vector.
      //DEPREC Joint (const Transform3f& initialPosition, size_type configSize,
      //DEPREC	     size_type numberDof);
      //DEPREC /// Copy constructor
      //DEPREC ///
      //DEPREC /// Clone body and therefore inner and outer objects (see Body::clone).
      //DEPREC Joint (const Joint& joint);
      //DEPREC /// Return pointer to copy of this
      //DEPREC ///
      //DEPREC /// Clone body and therefore inner and outer objects (see Body::clone).
      //DEPREC  virtual JointPtr_t clone () const = 0;
      //DEPREC virtual ~Joint ();

      ~Joint() {}
      /// \}
      // -----------------------------------------------------------------------
      /// \name Name
      /// \{

      //DEPREC /// Set name
      //DEPREC virtual inline void name(const std::string& name)

      /// Get name
      const std::string& name() const;

      /// \}
      // -----------------------------------------------------------------------
      /// \name Position
      /// \{

      //DEPREC /// Joint initial position (when robot is in zero configuration)
      //DEPREC const Transform3f& initialPosition () const;

      /// Joint transformation
      const Transform3f& currentTransformation () const;

      //DEPREC /// Compute position of joint
      //DEPREC /// \param configuration the configuration of the robot,
      //DEPREC /// \param parentPosition position of parent joint,
      //DEPREC /// \retval position position of this joint.
      //DEPREC virtual void computePosition (ConfigurationIn_t configuration,
      //DEPREC 				    const Transform3f& parentPosition,
      //DEPREC 				    Transform3f& position) const = 0;
      //DEPREC /// Compute position of this joint and all its descendents.
      //DEPREC void recursiveComputePosition (ConfigurationIn_t configuration,
      //DEPREC                const Transform3f& parentPosition) const;
      //DEPREC /// Compute jacobian matrix of joint and all its descendents.
      //DEPREC void computeJacobian ();

Nicolas Mansard's avatar
Nicolas Mansard committed
114
115
      //DEPREC /// Get neutral configuration of joint
      //DEPREC vector_t neutralConfiguration () const;
116
117
118
119
120
121
122

      ///\}
      // -----------------------------------------------------------------------
      /// \name Size and rank
      ///\}

      /// Return number of degrees of freedom
Nicolas Mansard's avatar
Nicolas Mansard committed
123
      size_type numberDof () const;
124
125

      /// Return number of degrees of freedom
Nicolas Mansard's avatar
Nicolas Mansard committed
126
      size_type configSize () const;
127
128

      /// Return rank of the joint in the configuration vector
Nicolas Mansard's avatar
Nicolas Mansard committed
129
      size_type rankInConfiguration () const;
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
159
160
161
162
163
164
165
166
167
168
169
170
171

      /// Return rank of the joint in the velocity vector
      size_type rankInVelocity () const;

      ///\}
      // -----------------------------------------------------------------------
      /// \name Kinematic chain
      /// \{

      /// Get a pointer to the parent joint (if any).
      // DEPREC JointPtr_t parentJoint () const

      //DEPREC /// Add child joint
      //DEPREC /// \param joint child joint added to this one,
      //DEPREC /// \param computePositionInParent whether to compute position of the
      //DEPREC ///        child joint in this one's frame.
      //DEPREC ///
      //DEPREC /// \note When building a kinematic chain, we usually build the
      //DEPREC /// joint in its initial position and compute the (constant)
      //DEPREC /// position of the joint in its parent when adding the joint in
      //DEPREC /// the kinematic chain. When copying a kinematic chain, we copy the
      //DEPREC /// position of the joint in its parent frame and therefore we do
      //DEPREC /// not update it when adding the joint in the kinematic chain.
      //DEPREC void addChildJoint (JointPtr_t joint,
      //DEPREC			  bool computePositionInParent = true);

      /// Number of child joints
      std::size_t numberChildJoints () const;

      /// Get child joint
      JointPtr_t childJoint (std::size_t rank) const;

      /// Get (constant) placement of joint in parent frame, i.e. model.jointPlacement[idx]
      const Transform3f& positionInParentFrame () const;

      //DEPREC /// Set position of joint in parent frame
      //DEPREC void positionInParentFrame (const Transform3f& p);
      ///\}
      // -----------------------------------------------------------------------
      /// \name Bounds
      /// \{

172
173
174
175
176
177
178
179
180
181
182
183
184
185
      /// Set whether given degree of freedom is bounded
      /// \warning Joint always has bounds. When `bounded == false`,
      /// the bounds are `-infinity` and `infinity`.
      void isBounded (size_type rank, bool bounded);
      /// Get whether given degree of freedom is bounded
      bool isBounded (size_type rank) const;
      /// Get lower bound of given degree of freedom
      value_type lowerBound (size_type rank) const;
      /// Get upper bound of given degree of freedom
      value_type upperBound (size_type rank) const;
      /// Set lower bound of given degree of freedom
      void lowerBound (size_type rank, value_type lowerBound);
      /// Set upper bound of given degree of freedom
      void upperBound (size_type rank, value_type upperBound);
186
187
188
189
190
191
192
193
194
195

      /// Get upper bound on linear velocity of the joint frame
      /// \return coefficient \f$\lambda\f$ such that
      /// \f{equation*}
      /// \forall \mathbf{q}_{joint}\ \ \ \ \ \ \|\mathbf {v}\| \leq \lambda \|\mathbf{\dot{q}}_{joint}\|
      /// \f}
      /// where
      /// \li \f$\mathbf{q}_{joint}\f$ is any joint configuration,
      /// \li \f$\mathbf{\dot{q}}_{joint}\f$ is the joint velocity, and
      /// \li \f$\mathbf{v} = J(\mathbf{q})*\mathbf{\dot{q}} \f$ is the linear velocity of the joint frame.
Nicolas Mansard's avatar
Nicolas Mansard committed
196
//NOTYET      value_type upperBoundLinearVelocity () const;
197
198
199
200
201
202
203
204
205
206

      /// Get upper bound on angular velocity of the joint frame
      /// \return coefficient \f$\lambda\f$ such that
      /// \f{equation*}
      /// \forall \mathbf{q}_{joint}\ \ \ \ \ \ \|\omega\| \leq \lambda \|\mathbf{\dot{q}}_{joint}\|
      /// \f}
      /// where
      /// \li \f$\mathbf{q}_{joint}\f$ is any joint configuration,
      /// \li \f$\mathbf{\dot{q}}_{joint}\f$ is the joint velocity, and
      /// \li \f$\omega = J(\mathbf{q})*\mathbf{\dot{q}}\f$ is the angular velocity of the joint frame.
Nicolas Mansard's avatar
Nicolas Mansard committed
207
//NOTYET      value_type upperBoundAngularVelocity () const;
208
209

      /// Maximal distance of joint origin to parent origin
Nicolas Mansard's avatar
Nicolas Mansard committed
210
//NOTYET      const value_type& maximalDistanceToParent () const;
211
      /// Compute the maximal distance. \sa maximalDistanceToParent
Nicolas Mansard's avatar
Nicolas Mansard committed
212
//NOTYET      void computeMaximalDistanceToParent ();
213
214
215
216
217
218
219

      /// \}
      // -----------------------------------------------------------------------
      /// \name Jacobian
      /// \{

      /// Get const reference to Jacobian
Nicolas Mansard's avatar
Nicolas Mansard committed
220
221
222
223
224
      /// \param localFrame if true, compute the jacobian (6d) in the local frame, 
      /// whose linear part corresponds to the velocity of the center of the frame.
      /// If false, the jacobian is expressed in the global frame and its linear part
      /// corresponds to the value of the velocity vector field at the center of the world.
      const JointJacobian_t& jacobian (const bool localFrame=true) const;
225
226

      /// Get non const reference to Jacobian
Nicolas Mansard's avatar
Nicolas Mansard committed
227
228
229
230
231
      /// \param localFrame if true, compute the jacobian (6d) in the local frame, 
      /// whose linear part corresponds to the velocity of the center of the frame.
      /// If false, the jacobian is expressed in the global frame and its linear part
      /// corresponds to the value of the velocity vector field at the center of the world.
      JointJacobian_t& jacobian (const bool localFrame=true);
232
233
234
235
236
237
238
239
240
241

      /// \}
      // -----------------------------------------------------------------------

      //DEPREC /// Access to configuration space
      //DEPREC JointConfiguration* configuration () const {return configuration_;}
      //DEPREC /// Set robot owning the kinematic chain
      //DEPREC void robot (const DeviceWkPtr_t& device) {robot_ = device;}

      /// Access robot owning the object
242
      DeviceConstPtr_t robot () const { selfAssert();  return devicePtr;}
243
      /// Access robot owning the object
244
      DevicePtr_t robot () { selfAssert(); return devicePtr;}
245
246
247
248
249

      /// \name Body linked to the joint
      /// \{

      /// Get linked body
250
      BodyPtr_t linkedBody () const;
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278

      //DEPREC /// Set linked body
      //DEPREC void setLinkedBody (const BodyPtr_t& body);

      /// \}

      //DEPREC /// \name Compatibility with urdf
      //DEPREC /// \{

      //DEPREC /// Get urdf link position in joint frame
      //DEPREC ///
      //DEPREC /// When parsing urdf pinocchios, joint frames are reoriented in order
      //DEPREC /// to rotate about their x-axis. For some applications, it is necessary
      //DEPREC /// to be able to recover the position of the urdf link attached to
      //DEPREC /// the joint.
      //DEPREC const Transform3f& linkInJointFrame () const;
      //DEPREC /// Set urdf link position in joint frame
      //DEPREC void linkInJointFrame (const Transform3f& transform);
      //DEPREC /// Get link name
      //DEPREC const std::string& linkName () const;
      //DEPREC /// Set link name
      //DEPREC void linkName (const std::string& linkName)

      //DEPREC /// \}

      /// Display joint
      virtual std::ostream& display (std::ostream& os) const;

Joseph Mirabel's avatar
Joseph Mirabel committed
279
280
281
282
283
284
285
286
      /// \name Pinocchio API
      /// \{

      const Index& index () const
      {
        return jointIndex;
      }

287
288
289
290
291
      se3::JointModel& jointModel()
      {
        return model()->joints[index()];
      }

Joseph Mirabel's avatar
Joseph Mirabel committed
292
293
      /// \}

294
295
296
    protected:
      value_type maximalDistanceToParent_;
      vector_t neutralConfiguration_;
297
      DevicePtr_t devicePtr;
Nicolas Mansard's avatar
Nicolas Mansard committed
298
      mutable JointJacobian_t jacobian_;
299
      Index jointIndex;
Nicolas Mansard's avatar
Nicolas Mansard committed
300
301
302
303
304
305
306
307
      std::vector<Index> children;

      /// Store list of childrens.
      void setChildList();
      ModelPtr_t       model() ;      
      ModelConstPtr_t  model() const ;
      DataPtr_t        data()  ;      
      DataConstPtr_t   data()  const ;
308

309
310
311
      /// Assert that the members of the struct are valid (no null pointer, etc).
      void selfAssert() const;

312
313
314
      friend class Device;
    }; // class Joint

Nicolas Mansard's avatar
Nicolas Mansard committed
315
    inline std::ostream& operator<< (std::ostream& os, const Joint& joint) { return joint.display(os); }
316

317
318
319
320
321
322
323
324
    /** Fake std::vector<Joint>, used to comply with the actual structure of hpp::model.
     *
     * You can use it for the following loop:
     *       for (JointVector_t::const_iterator it = jv.begin (); 
     *               it != jv.end (); ++it) 
     *          cout << (*it)->name;
     */
    struct JointVector
325
      : public FakeContainer<JointPtr_t,JointConstPtr_t>
326
    {
327
      JointVector(DevicePtr_t device) : FakeContainer<JointPtr_t,JointConstPtr_t>(device) {}
328
329
330
      JointVector() {}
      virtual ~JointVector() {}

331
332
      virtual JointPtr_t at(const size_type i) ;
      virtual JointConstPtr_t at(const size_type i) const ;
333
334
      virtual size_type size() const ;
      virtual size_type iend() const ;
335

336
337
      void selfAssert(size_type i = 0) const;
    };
338

339
340
341
342
  } // namespace pinocchio
} // namespace hpp

#endif // HPP_PINOCCHIO_JOINT_HH