hpp-pinocchio  4.12.0
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 
96 
99 
102 
104  // -----------------------------------------------------------------------
107 
110 
112  std::size_t numberChildJoints () const;
113 
115  JointPtr_t childJoint (std::size_t rank) const;
116 
119 
121  // -----------------------------------------------------------------------
124 
128  void isBounded (size_type rank, bool bounded);
130  bool isBounded (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 
154 
165 
168  if (maximalDistanceToParent_ < 0)
169  computeMaximalDistanceToParent();
170  return maximalDistanceToParent_;
171  }
172 
174  protected:
177  public:
178  // -----------------------------------------------------------------------
181 
187  JointJacobian_t& jacobian (const bool localFrame=true) const { return jacobian (data(), localFrame); }
188 
195  JointJacobian_t& jacobian (DeviceData& data, const bool localFrame=true) const;
196 
198  // -----------------------------------------------------------------------
199 
201  DeviceConstPtr_t robot () const { return devicePtr.lock();}
203  DevicePtr_t robot () { return devicePtr.lock();}
204 
207 
210 
212 
214  std::ostream& display (std::ostream& os) const;
215 
218 
222 
225 
226  const JointIndex& index () const
227  {
228  return jointIndex;
229  }
230 
231  const JointModel& jointModel() const;
232 
234 
235  bool operator== (const Joint& other) const
236  {
237  return index() == other.index() && robot() == other.robot();
238  }
239 
240  bool operator!= (const Joint& other) const
241  {
242  return !operator==(other);
243  }
244 
245  protected:
247  DeviceWkPtr_t devicePtr;
249  std::vector<JointIndex> children;
250 
252  void setChildList();
253  Model& model() ;
254  const Model& model() const ;
255  DeviceData& data() const;
256 
258  void selfAssert() const;
259 
260  friend class Device;
261 
262  Joint() {}
264  }; // class Joint
265 
266  inline std::ostream& operator<< (std::ostream& os, const Joint& joint) { return joint.display(os); }
267 
268  } // namespace pinocchio
269 } // namespace hpp
270 
271 #endif // HPP_PINOCCHIO_JOINT_HH
Robot with geometric and dynamic pinocchio.
Definition: device.hh:53
Definition: joint.hh:47
JointJacobian_t & jacobian(DeviceData &data, const bool localFrame=true) const
void isBounded(size_type rank, bool bounded)
value_type maximalDistanceToParent_
Definition: joint.hh:246
static JointPtr_t create(DeviceWkPtr_t device, JointIndex indexInJointList)
JointPtr_t childJoint(std::size_t rank) const
Get child joint.
std::ostream & display(std::ostream &os) const
Display joint.
void upperBound(size_type rank, value_type upperBound)
Set upper bound of given degree of freedom.
const value_type & maximalDistanceToParent()
Maximal distance of joint origin to parent origin.
Definition: joint.hh:167
value_type upperBound(size_type rank) const
Get upper bound of given degree of freedom.
DeviceData & data() const
DevicePtr_t robot()
Access robot owning the object.
Definition: joint.hh:203
const Model & model() const
value_type lowerBound(size_type rank) const
Get lower bound of given degree of freedom.
const Transform3f & positionInParentFrame() const
Get (constant) placement of joint in parent frame, i.e. model.jointPlacement[idx].
size_type numberDof() const
Return number of degrees of freedom.
BodyPtr_t linkedBody() const
Get linked body.
void computeMaximalDistanceToParent()
Compute the maximal distance.
std::vector< JointIndex > children
Definition: joint.hh:249
Joint()
Definition: joint.hh:262
JointPtr_t parentJoint() const
Get a pointer to the parent joint (if any).
void lowerBound(size_type rank, value_type lowerBound)
Set lower bound of given degree of freedom.
~Joint()
Definition: joint.hh:66
const Transform3f & currentTransformation(const DeviceData &data) const
Joint transformation.
std::size_t numberChildJoints() const
Number of child joints.
size_type rankInVelocity() const
Return rank of the joint in the velocity vector.
void lowerBounds(vectorIn_t lowerBounds)
Set lower bounds.
const std::string & name() const
Get name.
DeviceConstPtr_t robot() const
Access robot owning the object.
Definition: joint.hh:201
size_type rankInConfiguration() const
Return rank of the joint in the configuration vector.
value_type upperBoundLinearVelocity() const
void selfAssert() const
Assert that the members of the struct are valid (no null pointer, etc).
LiegroupSpacePtr_t RnxSOnConfigurationSpace() const
Joint(DeviceWkPtr_t device, JointIndex indexInJointList)
void setChildList()
Store list of childrens.
value_type upperBoundAngularVelocity() const
const JointIndex & index() const
Definition: joint.hh:226
void upperBounds(vectorIn_t upperBounds)
Set upper bounds.
DeviceWkPtr_t devicePtr
Definition: joint.hh:247
size_type configSize() const
Return number of degrees of freedom.
JointIndex jointIndex
Definition: joint.hh:248
const JointModel & jointModel() const
JointJacobian_t & jacobian(const bool localFrame=true) const
Definition: joint.hh:187
bool isBounded(size_type rank) const
Get whether given degree of freedom is bounded.
LiegroupSpacePtr_t configurationSpace() const
Get configuration space of joint.
const Transform3f & currentTransformation() const
Joint transformation.
Definition: joint.hh:81
#define HPP_PINOCCHIO_DLLAPI
Definition: config.hh:64
shared_ptr< Device > DevicePtr_t
Definition: fwd.hh:106
shared_ptr< Joint > JointPtr_t
Definition: fwd.hh:111
::pinocchio::ModelTpl< value_type, 0, JointCollectionTpl > Model
Definition: fwd.hh:65
std::ostream & operator<<(std::ostream &os, const hpp::pinocchio::Device &device)
Definition: device.hh:359
shared_ptr< LiegroupSpace > LiegroupSpacePtr_t
Definition: fwd.hh:136
::pinocchio::SE3 Transform3f
Definition: fwd.hh:69
matrix_t::Index size_type
Definition: fwd.hh:84
Eigen::Matrix< value_type, 6, Eigen::Dynamic > JointJacobian_t
Definition: fwd.hh:88
::pinocchio::JointModelTpl< value_type, 0, JointCollectionTpl > JointModel
Definition: fwd.hh:71
shared_ptr< Body > BodyPtr_t
Definition: fwd.hh:96
double value_type
Definition: fwd.hh:40
::pinocchio::JointIndex JointIndex
Definition: fwd.hh:62
shared_ptr< const Device > DeviceConstPtr_t
Definition: fwd.hh:107
Eigen::Ref< const vector_t > vectorIn_t
Definition: fwd.hh:80
Utility functions.
Definition: body.hh:30
Definition: collision-object.hh:32
Definition: device-data.hh:42