DART  6.7.3
TemplatedJacobianNode.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_DETAIL_TEMPLATEDJACOBIAN_HPP_
34 #define DART_DYNAMICS_DETAIL_TEMPLATEDJACOBIAN_HPP_
35 
37 
38 namespace dart {
39 namespace dynamics {
40 
41 //==============================================================================
42 template<class NodeType>
45  const Frame* _inCoordinatesOf) const
46 {
47  if(this == _inCoordinatesOf)
48  return static_cast<const NodeType*>(this)->getJacobian();
49  else if(_inCoordinatesOf->isWorld())
50  return static_cast<const NodeType*>(this)->getWorldJacobian();
51 
52  return math::AdRJac(getTransform(_inCoordinatesOf),
53  static_cast<const NodeType*>(this)->getJacobian());
54 }
55 
56 //==============================================================================
57 template<class NodeType>
60  const Eigen::Vector3d& _offset) const
61 {
62  math::Jacobian J = static_cast<const NodeType*>(this)->getJacobian();
63  J.bottomRows<3>() += J.topRows<3>().colwise().cross(_offset);
64 
65  return J;
66 }
67 
68 //==============================================================================
69 template<class NodeType>
72  const Eigen::Vector3d& _offset,
73  const Frame* _inCoordinatesOf) const
74 {
75  if(this == _inCoordinatesOf)
76  return getJacobian(_offset);
77  else if(_inCoordinatesOf->isWorld())
78  return getWorldJacobian(_offset);
79 
80  Eigen::Isometry3d T = getTransform(_inCoordinatesOf);
81  T.translation() = - T.linear() * _offset;
82 
83  return math::AdTJac(T, static_cast<const NodeType*>(this)->getJacobian());
84 }
85 
86 //==============================================================================
87 template<class NodeType>
90  const Eigen::Vector3d& _offset) const
91 {
92  math::Jacobian J = static_cast<const NodeType*>(this)->getWorldJacobian();
93  J.bottomRows<3>() += J.topRows<3>().colwise().cross(
94  getWorldTransform().linear() * _offset);
95 
96  return J;
97 }
98 
99 //==============================================================================
100 template<class NodeType>
103  const Frame* _inCoordinatesOf) const
104 {
105  if(this == _inCoordinatesOf)
106  {
107  const math::Jacobian& J =
108  static_cast<const NodeType*>(this)->getJacobian();
109 
110  return J.bottomRows<3>();
111  }
112  else if(_inCoordinatesOf->isWorld())
113  {
114  const math::Jacobian& JWorld =
115  static_cast<const NodeType*>(this)->getWorldJacobian();
116 
117  return JWorld.bottomRows<3>();
118  }
119 
120  const math::Jacobian& J =
121  static_cast<const NodeType*>(this)->getJacobian();
122 
123  return getTransform(_inCoordinatesOf).linear() * J.bottomRows<3>();
124 }
125 
126 //==============================================================================
127 template<class NodeType>
130  const Eigen::Vector3d& _offset,
131  const Frame* _inCoordinatesOf) const
132 {
133  const math::Jacobian& J =
134  static_cast<const NodeType*>(this)->getJacobian();
135 
136  math::LinearJacobian JLinear;
137  JLinear = J.bottomRows<3>() + J.topRows<3>().colwise().cross(_offset);
138 
139  if(this == _inCoordinatesOf)
140  return JLinear;
141 
142  return getTransform(_inCoordinatesOf).linear() * JLinear;
143 }
144 
145 //==============================================================================
146 template<class NodeType>
149  const Frame* _inCoordinatesOf) const
150 {
151  if(this == _inCoordinatesOf)
152  {
153  const math::Jacobian& J =
154  static_cast<const NodeType*>(this)->getJacobian();
155  return J.topRows<3>();
156  }
157  else if(_inCoordinatesOf->isWorld())
158  {
159  const math::Jacobian& JWorld =
160  static_cast<const NodeType*>(this)->getWorldJacobian();
161  return JWorld.topRows<3>();
162  }
163 
164  const math::Jacobian& J =
165  static_cast<const NodeType*>(this)->getJacobian();
166 
167  return getTransform(_inCoordinatesOf).linear() * J.topRows<3>();
168 }
169 
170 //==============================================================================
171 template<class NodeType>
174  const Frame* _inCoordinatesOf) const
175 {
176  if(this == _inCoordinatesOf)
177  return static_cast<const NodeType*>(this)->getJacobianSpatialDeriv();
178 
179  return math::AdRJac(getTransform(_inCoordinatesOf),
180  static_cast<const NodeType*>(this)->getJacobianSpatialDeriv());
181 }
182 
183 //==============================================================================
184 template<class NodeType>
187  const Eigen::Vector3d& _offset) const
188 {
189  math::Jacobian J_d =
190  static_cast<const NodeType*>(this)->getJacobianSpatialDeriv();
191 
192  J_d.bottomRows<3>() += J_d.topRows<3>().colwise().cross(_offset);
193 
194  return J_d;
195 }
196 
197 //==============================================================================
198 template<class NodeType>
201  const Eigen::Vector3d& _offset,
202  const Frame* _inCoordinatesOf) const
203 {
204  if(this == _inCoordinatesOf)
205  return getJacobianSpatialDeriv(_offset);
206 
207  Eigen::Isometry3d T = getTransform(_inCoordinatesOf);
208  T.translation() = T.linear() * -_offset;
209 
210  return math::AdTJac(
211  T, static_cast<const NodeType*>(this)->getJacobianSpatialDeriv());
212 }
213 
214 //==============================================================================
215 template<class NodeType>
218  const Frame* _inCoordinatesOf) const
219 {
220  if(_inCoordinatesOf->isWorld())
221  return static_cast<const NodeType*>(this)->getJacobianClassicDeriv();
222 
223  return math::AdRInvJac(_inCoordinatesOf->getWorldTransform(),
224  static_cast<const NodeType*>(this)->getJacobianClassicDeriv());
225 }
226 
227 //==============================================================================
228 template<class NodeType>
231  const Eigen::Vector3d& _offset,
232  const Frame* _inCoordinatesOf) const
233 {
234  math::Jacobian J_d =
235  static_cast<const NodeType*>(this)->getJacobianClassicDeriv();
236 
237  const math::Jacobian& J =
238  static_cast<const NodeType*>(this)->getWorldJacobian();
239 
240  const Eigen::Vector3d& w = getAngularVelocity();
241  const Eigen::Vector3d& p = (getWorldTransform().linear() * _offset).eval();
242 
243  J_d.bottomRows<3>() += J_d.topRows<3>().colwise().cross(p)
244  + J.topRows<3>().colwise().cross(w.cross(p));
245 
246  if(_inCoordinatesOf->isWorld())
247  return J_d;
248 
249  return math::AdRInvJac(_inCoordinatesOf->getWorldTransform(), J_d);
250 }
251 
252 //==============================================================================
253 template<class NodeType>
256  const Frame* _inCoordinatesOf) const
257 {
258  const math::Jacobian& J_d =
259  static_cast<const NodeType*>(this)->getJacobianClassicDeriv();
260 
261  if(_inCoordinatesOf->isWorld())
262  return J_d.bottomRows<3>();
263 
264  return _inCoordinatesOf->getWorldTransform().linear().transpose()
265  * J_d.bottomRows<3>();
266 }
267 
268 //==============================================================================
269 template<class NodeType>
272  const Eigen::Vector3d& _offset,
273  const Frame* _inCoordinatesOf) const
274 {
275  const math::Jacobian& J_d =
276  static_cast<const NodeType*>(this)->getJacobianClassicDeriv();
277 
278  const math::Jacobian& J =
279  static_cast<const NodeType*>(this)->getWorldJacobian();
280 
281  const Eigen::Vector3d& w = getAngularVelocity();
282  const Eigen::Vector3d& p = (getWorldTransform().linear() * _offset).eval();
283 
284  if(_inCoordinatesOf->isWorld())
285  return J_d.bottomRows<3>() + J_d.topRows<3>().colwise().cross(p)
286  + J.topRows<3>().colwise().cross(w.cross(p));
287 
288  return _inCoordinatesOf->getWorldTransform().linear().transpose()
289  * (J_d.bottomRows<3>() + J_d.topRows<3>().colwise().cross(p)
290  + J.topRows<3>().colwise().cross(w.cross(p)));
291 }
292 
293 //==============================================================================
294 template<class NodeType>
297  const Frame* _inCoordinatesOf) const
298 {
299  const math::Jacobian& J_d =
300  static_cast<const NodeType*>(this)->getJacobianClassicDeriv();
301 
302  if(_inCoordinatesOf->isWorld())
303  return J_d.topRows<3>();
304 
305  return _inCoordinatesOf->getWorldTransform().linear().transpose()
306  * J_d.topRows<3>();
307 }
308 
309 //==============================================================================
310 template<class NodeType>
312  : Entity(Entity::ConstructAbstract),
313  Frame(Frame::ConstructAbstract),
314  JacobianNode(bn)
315 {
316  // Do nothing
317 }
318 
319 } // namespace dynamics
320 } // namespace dart
321 
322 
323 #endif // DART_DYNAMICS_DETAIL_TEMPLATEDJACOBIAN_HPP_
BodyNode class represents a single node of the skeleton.
Definition: BodyNode.hpp:78
Entity class is a base class for any objects that exist in the kinematic tree structure of DART.
Definition: Entity.hpp:60
The Frame class serves as the backbone of DART's kinematic tree structure.
Definition: Frame.hpp:57
bool isWorld() const
Returns true if this Frame is the World Frame.
Definition: Frame.cpp:445
const Eigen::Isometry3d & getWorldTransform() const
Get the transform of this Frame with respect to the World Frame.
Definition: Frame.cpp:79
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 const math::Jacobian & getJacobianClassicDeriv() const =0
Return the classical time derivative of the generalized Jacobian targeting the origin of this BodyNod...
virtual const math::Jacobian & getJacobianSpatialDeriv() const =0
Return the spatial time derivative of the generalized Jacobian targeting the origin of this BodyNode.
virtual const math::Jacobian & getWorldJacobian() const =0
Return the generalized Jacobian targeting the origin of this JacobianNode.
math::AngularJacobian getAngularJacobian(const Frame *_inCoordinatesOf=Frame::World()) const override final
Return the angular Jacobian targeting the origin of this BodyNode.
Definition: TemplatedJacobianNode.hpp:148
TemplatedJacobianNode(BodyNode *bn)
Constructor.
Definition: TemplatedJacobianNode.hpp:311
math::LinearJacobian getLinearJacobian(const Frame *_inCoordinatesOf=Frame::World()) const override final
Return the linear Jacobian targeting the origin of this BodyNode.
Definition: TemplatedJacobianNode.hpp:102
math::AngularJacobian getAngularJacobianDeriv(const Frame *_inCoordinatesOf=Frame::World()) const override final
Return the angular Jacobian time derivative, in terms of any coordinate Frame.
Definition: TemplatedJacobianNode.hpp:296
math::LinearJacobian getLinearJacobianDeriv(const Frame *_inCoordinatesOf=Frame::World()) const override final
Return the linear Jacobian (classical) time derivative, in terms of any coordinate Frame.
Definition: TemplatedJacobianNode.hpp:255
Derived::PlainObject AdRJac(const Eigen::Isometry3d &_T, const Eigen::MatrixBase< Derived > &_J)
Change coordinate Frame of a Jacobian.
Definition: Geometry.hpp:271
Eigen::Matrix< double, 6, Eigen::Dynamic > Jacobian
Definition: MathTypes.hpp:108
Eigen::Matrix< double, 3, Eigen::Dynamic > AngularJacobian
Definition: MathTypes.hpp:107
Derived::PlainObject AdRInvJac(const Eigen::Isometry3d &_T, const Eigen::MatrixBase< Derived > &_J)
Definition: Geometry.hpp:289
Eigen::Matrix< double, 3, Eigen::Dynamic > LinearJacobian
Definition: MathTypes.hpp:106
Derived::PlainObject AdTJac(const Eigen::Isometry3d &_T, const Eigen::MatrixBase< Derived > &_J)
Adjoint mapping for dynamic size Jacobian.
Definition: Geometry.hpp:216
Definition: BulletCollisionDetector.cpp:63