hpp-pinocchio  4.10.1
Wrapping of the kinematic/dynamic chain Pinocchio for HPP.
joint.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2016 CNRS
3 // Author: NMansard from Florent Lamiraux
4 //
5 //
6 // This file is part of hpp-pinocchio
7 // hpp-pinocchio is free software: you can redistribute it
8 // and/or modify it under the terms of the GNU Lesser General Public
9 // License as published by the Free Software Foundation, either version
10 // 3 of the License, or (at your option) any later version.
11 //
12 // hpp-pinocchio is distributed in the hope that it will be
13 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
14 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Lesser Public License for more details. You should have
16 // received a copy of the GNU Lesser General Public License along with
17 // hpp-pinocchio If not, see
18 // <http://www.gnu.org/licenses/>.
19 
20 #ifndef HPP_PINOCCHIO_JOINT_HH
21 # define HPP_PINOCCHIO_JOINT_HH
22 
23 # include <cstddef>
24 # include <hpp/pinocchio/fwd.hh>
25 # include <hpp/pinocchio/config.hh>
26 
27 # include <hpp/util/serialization-fwd.hh>
28 
29 namespace hpp {
30  namespace pinocchio {
48  public:
51 
58  static JointPtr_t create (DeviceWkPtr_t device, JointIndex indexInJointList );
59 
64  Joint (DeviceWkPtr_t device, JointIndex indexInJointList );
65 
66  ~Joint() {}
68  // -----------------------------------------------------------------------
71 
73  const std::string& name() const;
74 
76  // -----------------------------------------------------------------------
79 
81  const Transform3f& currentTransformation () const { return currentTransformation(data()); }
82 
84  const Transform3f& currentTransformation (const DeviceData& data) const;
85 
87  // -----------------------------------------------------------------------
90 
92  size_type numberDof () const;
93 
95  size_type configSize () const;
96 
98  size_type rankInConfiguration () const;
99 
101  size_type rankInVelocity () const;
102 
104  // -----------------------------------------------------------------------
107 
109  JointPtr_t parentJoint () const;
110 
112  std::size_t numberChildJoints () const;
113 
115  JointPtr_t childJoint (std::size_t rank) const;
116 
118  const Transform3f& positionInParentFrame () const;
119 
121  // -----------------------------------------------------------------------
124 
128  void isBounded (size_type rank, bool bounded);
130  bool isBounded (size_type rank) const;
132  value_type lowerBound (size_type rank) const;
134  value_type upperBound (size_type rank) const;
136  void lowerBound (size_type rank, value_type lowerBound);
138  void upperBound (size_type rank, value_type upperBound);
140  void lowerBounds (vectorIn_t lowerBounds);
142  void upperBounds (vectorIn_t upperBounds);
143 
153  value_type upperBoundLinearVelocity () const;
154 
164  value_type upperBoundAngularVelocity () const;
165 
167  const value_type& maximalDistanceToParent () const { return maximalDistanceToParent_; }
168 
170  protected:
172  void computeMaximalDistanceToParent ();
173  public:
174  // -----------------------------------------------------------------------
177 
183  JointJacobian_t& jacobian (const bool localFrame=true) const { return jacobian (data(), localFrame); }
184 
191  JointJacobian_t& jacobian (DeviceData& data, const bool localFrame=true) const;
192 
194  // -----------------------------------------------------------------------
195 
197  DeviceConstPtr_t robot () const { return devicePtr.lock();}
199  DevicePtr_t robot () { return devicePtr.lock();}
200 
203 
205  BodyPtr_t linkedBody () const;
206 
208 
210  std::ostream& display (std::ostream& os) const;
211 
213  LiegroupSpacePtr_t configurationSpace () const;
214 
217  LiegroupSpacePtr_t RnxSOnConfigurationSpace () const;
218 
221 
222  const JointIndex& index () const
223  {
224  return jointIndex;
225  }
226 
227  const JointModel& jointModel() const;
228 
230 
231  bool operator== (const Joint& other) const
232  {
233  return index() == other.index() && robot() == other.robot();
234  }
235 
236  bool operator!= (const Joint& other) const
237  {
238  return !operator==(other);
239  }
240 
241  protected:
243  DeviceWkPtr_t devicePtr;
245  std::vector<JointIndex> children;
246 
248  void setChildList();
249  Model& model() ;
250  const Model& model() const ;
251  DeviceData& data() const;
252 
254  void selfAssert() const;
255 
256  friend class Device;
257 
258  Joint() {}
259  HPP_SERIALIZABLE();
260  }; // class Joint
261 
262  inline std::ostream& operator<< (std::ostream& os, const Joint& joint) { return joint.display(os); }
263 
264  } // namespace pinocchio
265 } // namespace hpp
266 
267 #endif // HPP_PINOCCHIO_JOINT_HH
boost::shared_ptr< Device > DevicePtr_t
Definition: fwd.hh:106
boost::shared_ptr< Body > BodyPtr_t
Definition: fwd.hh:96
Utility functions.
Definition: body.hh:30
Eigen::Matrix< value_type, 6, Eigen::Dynamic > JointJacobian_t
Definition: fwd.hh:88
Definition: device-data.hh:41
const Transform3f & currentTransformation() const
Joint transformation.
Definition: joint.hh:81
::pinocchio::ModelTpl< value_type, 0, JointCollectionTpl > Model
Definition: fwd.hh:65
matrix_t::Index size_type
Definition: fwd.hh:84
JointIndex jointIndex
Definition: joint.hh:244
std::vector< JointIndex > children
Definition: joint.hh:245
std::ostream & operator<<(std::ostream &os, const hpp::pinocchio::Device &device)
Definition: device.hh:353
#define HPP_PINOCCHIO_DLLAPI
Definition: config.hh:64
const JointIndex & index() const
Definition: joint.hh:222
JointJacobian_t & jacobian(const bool localFrame=true) const
Definition: joint.hh:183
::pinocchio::JointModelTpl< value_type, 0, JointCollectionTpl > JointModel
Definition: fwd.hh:71
std::ostream & display(std::ostream &os) const
Display joint.
Robot with geometric and dynamic pinocchio.
Definition: device.hh:52
double value_type
Definition: fwd.hh:40
Definition: collision-object.hh:31
DeviceWkPtr_t devicePtr
Definition: joint.hh:243
::pinocchio::SE3 Transform3f
Definition: fwd.hh:69
boost::shared_ptr< const Device > DeviceConstPtr_t
Definition: fwd.hh:107
Eigen::Ref< const vector_t > vectorIn_t
Definition: fwd.hh:80
DevicePtr_t robot()
Access robot owning the object.
Definition: joint.hh:199
const value_type & maximalDistanceToParent() const
Maximal distance of joint origin to parent origin.
Definition: joint.hh:167
Joint()
Definition: joint.hh:258
~Joint()
Definition: joint.hh:66
::pinocchio::JointIndex JointIndex
Definition: fwd.hh:62
boost::shared_ptr< LiegroupSpace > LiegroupSpacePtr_t
Definition: fwd.hh:136
DeviceConstPtr_t robot() const
Access robot owning the object.
Definition: joint.hh:197
Definition: joint.hh:47
std::ostream & display(std::ostream &os, const SE3 &m)
Write a SE3 taking into account the indentation.
value_type maximalDistanceToParent_
Definition: joint.hh:242
boost::shared_ptr< Joint > JointPtr_t
Definition: fwd.hh:111