DART  6.7.3
JacobianNode.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2019, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_DYNAMICS_JACOBIANNODE_HPP_
34 #define DART_DYNAMICS_JACOBIANNODE_HPP_
35 
36 #include <memory>
37 #include <unordered_set>
38 
39 #include "dart/dynamics/Frame.hpp"
40 #include "dart/dynamics/Node.hpp"
42 
43 namespace dart {
44 namespace dynamics {
45 
46 class Skeleton;
47 class DegreeOfFreedom;
48 class InverseKinematics;
49 
53 class JacobianNode : public virtual Frame, public Node
54 {
55 public:
56 
58  virtual ~JacobianNode();
59 
60  using Node::setName;
61  using Node::getName;
62 
65  const std::shared_ptr<InverseKinematics>& getIK(bool _createIfNull = false);
66 
70  const std::shared_ptr<InverseKinematics>& getOrCreateIK();
71 
75  std::shared_ptr<const InverseKinematics> getIK() const;
76 
80  const std::shared_ptr<InverseKinematics>& createIK();
81 
83  void clearIK();
84 
85  //----------------------------------------------------------------------------
87  //----------------------------------------------------------------------------
88 
90  virtual bool dependsOn(std::size_t _genCoordIndex) const = 0;
91 
93  virtual std::size_t getNumDependentGenCoords() const = 0;
94 
97  virtual std::size_t getDependentGenCoordIndex(std::size_t _arrayIndex) const = 0;
98 
100  virtual const std::vector<std::size_t>& getDependentGenCoordIndices() const = 0;
101 
103  virtual std::size_t getNumDependentDofs() const = 0;
104 
106  virtual DegreeOfFreedom* getDependentDof(std::size_t _index) = 0;
107 
109  virtual const DegreeOfFreedom* getDependentDof(std::size_t _index) const = 0;
110 
112  virtual const std::vector<DegreeOfFreedom*>& getDependentDofs() = 0;
113 
115  virtual const std::vector<const DegreeOfFreedom*>& getDependentDofs() const = 0;
116 
119  virtual const std::vector<const DegreeOfFreedom*> getChainDofs() const = 0;
120 
122 
123  //----------------------------------------------------------------------------
125  //----------------------------------------------------------------------------
126 
129  virtual const math::Jacobian& getJacobian() const = 0;
130 
133  virtual math::Jacobian getJacobian(const Frame* _inCoordinatesOf) const = 0;
134 
137  virtual math::Jacobian getJacobian(const Eigen::Vector3d& _offset) const = 0;
138 
141  virtual math::Jacobian getJacobian(const Eigen::Vector3d& _offset,
142  const Frame* _inCoordinatesOf) const = 0;
143 
146  virtual const math::Jacobian& getWorldJacobian() const = 0;
147 
152  const Eigen::Vector3d& _offset) const = 0;
153 
157  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
158 
162  const Eigen::Vector3d& _offset,
163  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
164 
168  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
169 
178  virtual const math::Jacobian& getJacobianSpatialDeriv() const = 0;
179 
188  const Frame* _inCoordinatesOf) const = 0;
189 
201  const Eigen::Vector3d& _offset) const = 0;
202 
206  const Eigen::Vector3d& _offset,
207  const Frame* _inCoordinatesOf) const = 0;
208 
216  virtual const math::Jacobian& getJacobianClassicDeriv() const = 0;
217 
225  const Frame* _inCoordinatesOf) const = 0;
226 
235  const Eigen::Vector3d& _offset,
236  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
237 
245  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
246 
255  const Eigen::Vector3d& _offset,
256  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
257 
261  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
262 
264 
267  DART_DEPRECATED(6.2)
268  void notifyJacobianUpdate();
269 
272  void dirtyJacobian();
273 
276  DART_DEPRECATED(6.2)
278 
281  void dirtyJacobianDeriv();
282 
283 protected:
284 
286  JacobianNode(BodyNode* bn);
287 
289  mutable bool mIsBodyJacobianDirty;
290 
292  mutable bool mIsWorldJacobianDirty;
293 
296 
299 
301  std::shared_ptr<InverseKinematics> mIK;
302 
305 
306 };
307 
308 } // namespace dart
309 } // namespace dynamics
310 
311 #endif // DART_DYNAMICS_JACOBIANNODE_HPP_
#define DART_DEPRECATED(version)
Definition: Deprecated.hpp:51
BodyNode class represents a single node of the skeleton.
Definition: BodyNode.hpp:78
DegreeOfFreedom class is a proxy class for accessing single degrees of freedom (aka generalized coord...
Definition: DegreeOfFreedom.hpp:53
The Frame class serves as the backbone of DART's kinematic tree structure.
Definition: Frame.hpp:57
static Frame * World()
Definition: Frame.cpp:72
The InverseKinematics class provides a convenient way of setting up an IK optimization problem.
Definition: InverseKinematics.hpp:76
The JacobianNode class serves as a common interface for BodyNodes and EndEffectors to both be used as...
Definition: JacobianNode.hpp:54
virtual const math::Jacobian & getJacobian() const =0
Return the generalized Jacobian targeting the origin of this JacobianNode.
virtual math::Jacobian getJacobian(const Eigen::Vector3d &_offset) const =0
Return the generalized Jacobian targeting an offset within the Frame of this JacobianNode.
virtual const math::Jacobian & getJacobianClassicDeriv() const =0
Return the classical time derivative of the generalized Jacobian targeting the origin of this BodyNod...
virtual math::Jacobian getJacobian(const Frame *_inCoordinatesOf) const =0
A version of getJacobian() that lets you specify a coordinate Frame to express the Jacobian in.
virtual math::AngularJacobian getAngularJacobian(const Frame *_inCoordinatesOf=Frame::World()) const =0
Return the angular Jacobian targeting the origin of this BodyNode.
virtual std::size_t getNumDependentGenCoords() const =0
The number of the generalized coordinates which affect this JacobianNode.
virtual math::Jacobian getJacobianSpatialDeriv(const Eigen::Vector3d &_offset, const Frame *_inCoordinatesOf) const =0
A version of getJacobianSpatialDeriv(const Eigen::Vector3d&) that allows an arbitrary coordinate Fram...
std::unordered_set< JacobianNode * > mChildJacobianNodes
JacobianNode children that descend from this JacobianNode.
Definition: JacobianNode.hpp:304
virtual const std::vector< const DegreeOfFreedom * > & getDependentDofs() const =0
Return a std::vector of DegreeOfFreedom pointers that this Node depends on.
virtual math::LinearJacobian getLinearJacobian(const Eigen::Vector3d &_offset, const Frame *_inCoordinatesOf=Frame::World()) const =0
Return the generalized Jacobian targeting an offset within the Frame of this BodyNode.
std::shared_ptr< const InverseKinematics > getIK() const
Get a pointer to an IK module for this JacobianNode.
Definition: JacobianNode.cpp:66
virtual math::Jacobian getJacobianSpatialDeriv(const Eigen::Vector3d &_offset) const =0
Return the spatial time derivative of the generalized Jacobian targeting an offset in the Frame of th...
virtual DegreeOfFreedom * getDependentDof(std::size_t _index)=0
Get a pointer to the _indexth dependent DegreeOfFreedom for this BodyNode.
virtual math::Jacobian getJacobianClassicDeriv(const Eigen::Vector3d &_offset, const Frame *_inCoordinatesOf=Frame::World()) const =0
A version of getJacobianClassicDeriv() that can compute the Jacobian for an offset within the Frame o...
virtual bool dependsOn(std::size_t _genCoordIndex) const =0
Return true if _genCoordIndex-th generalized coordinate.
std::shared_ptr< InverseKinematics > mIK
Inverse kinematics module which gets lazily created upon request.
Definition: JacobianNode.hpp:301
bool mIsBodyJacobianSpatialDerivDirty
Dirty flag for spatial time derivative of body Jacobian.
Definition: JacobianNode.hpp:295
virtual math::Jacobian getJacobianClassicDeriv(const Frame *_inCoordinatesOf) const =0
A version of getJacobianClassicDeriv() that can return the Jacobian in coordinates of any Frame.
virtual math::LinearJacobian getLinearJacobianDeriv(const Eigen::Vector3d &_offset, const Frame *_inCoordinatesOf=Frame::World()) const =0
A version of getLinearJacobianDeriv() that can compute the Jacobian for an offset within the Frame of...
const std::shared_ptr< InverseKinematics > & createIK()
Create a new IK module for this JacobianNode.
Definition: JacobianNode.cpp:72
virtual math::Jacobian getJacobian(const Eigen::Vector3d &_offset, const Frame *_inCoordinatesOf) const =0
A version of getJacobian(const Eigen::Vector3d&) that lets you specify a coordinate Frame to express ...
virtual const DegreeOfFreedom * getDependentDof(std::size_t _index) const =0
Get a pointer to the _indexth dependent DegreeOfFreedom for this BodyNode.
virtual math::Jacobian getJacobianSpatialDeriv(const Frame *_inCoordinatesOf) const =0
A version of getJacobianSpatialDeriv() that can return the Jacobian in coordinates of any Frame.
const std::shared_ptr< InverseKinematics > & getOrCreateIK()
Get a pointer to an IK module for this JacobianNode.
Definition: JacobianNode.cpp:60
void dirtyJacobian()
Notify this BodyNode and all its descendents that their Jacobians need to be updated.
Definition: JacobianNode.cpp:105
virtual const std::vector< DegreeOfFreedom * > & getDependentDofs()=0
Return a std::vector of DegreeOfFreedom pointers that this Node depends on.
virtual const math::Jacobian & getJacobianSpatialDeriv() const =0
Return the spatial time derivative of the generalized Jacobian targeting the origin of this BodyNode.
virtual math::LinearJacobian getLinearJacobian(const Frame *_inCoordinatesOf=Frame::World()) const =0
Return the linear Jacobian targeting the origin of this BodyNode.
virtual math::AngularJacobian getAngularJacobianDeriv(const Frame *_inCoordinatesOf=Frame::World()) const =0
Return the angular Jacobian time derivative, in terms of any coordinate Frame.
virtual math::LinearJacobian getLinearJacobianDeriv(const Frame *_inCoordinatesOf=Frame::World()) const =0
Return the linear Jacobian (classical) time derivative, in terms of any coordinate Frame.
void notifyJacobianUpdate()
Notify this BodyNode and all its descendents that their Jacobians need to be updated.
Definition: JacobianNode.cpp:99
virtual ~JacobianNode()
Virtual destructor.
Definition: JacobianNode.cpp:44
void dirtyJacobianDeriv()
Notify this BodyNode and all its descendents that their Jacobian derivatives need to be updated.
Definition: JacobianNode.cpp:126
virtual std::size_t getDependentGenCoordIndex(std::size_t _arrayIndex) const =0
Return a generalized coordinate index from the array index (< getNumDependentDofs)
bool mIsBodyJacobianDirty
Dirty flag for body Jacobian.
Definition: JacobianNode.hpp:289
virtual std::size_t getNumDependentDofs() const =0
Same as getNumDependentGenCoords()
void notifyJacobianDerivUpdate()
Notify this BodyNode and all its descendents that their Jacobian derivatives need to be updated.
Definition: JacobianNode.cpp:120
virtual const math::Jacobian & getWorldJacobian() const =0
Return the generalized Jacobian targeting the origin of this JacobianNode.
bool mIsWorldJacobianDirty
Dirty flag for world Jacobian.
Definition: JacobianNode.hpp:292
virtual const std::vector< std::size_t > & getDependentGenCoordIndices() const =0
Indices of the generalized coordinates which affect this JacobianNode.
virtual math::Jacobian getWorldJacobian(const Eigen::Vector3d &_offset) const =0
Return the generalized Jacobian targeting an offset in this JacobianNode.
bool mIsWorldJacobianClassicDerivDirty
Dirty flag for the classic time derivative of the Jacobian.
Definition: JacobianNode.hpp:298
void clearIK()
Wipe away the IK module for this JacobianNode, leaving it as a nullptr.
Definition: JacobianNode.cpp:79
virtual const std::vector< const DegreeOfFreedom * > getChainDofs() const =0
Returns a DegreeOfFreedom vector containing the dofs that form a Chain leading up to this JacobianNod...
The Node class is a base class for BodyNode and any object that attaches to a BodyNode.
Definition: Node.hpp:84
virtual const std::string & setName(const std::string &newName)=0
Set the name of this Node.
virtual const std::string & getName() const =0
Get the name of this Node.
Eigen::Matrix< double, 6, Eigen::Dynamic > Jacobian
Definition: MathTypes.hpp:108
Eigen::Matrix< double, 3, Eigen::Dynamic > AngularJacobian
Definition: MathTypes.hpp:107
Eigen::Matrix< double, 3, Eigen::Dynamic > LinearJacobian
Definition: MathTypes.hpp:106
Definition: BulletCollisionDetector.cpp:63
Definition: SharedLibraryManager.hpp:43