DART 6.7.3
Loading...
Searching...
No Matches
FreeJoint.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_FREEJOINT_HPP_
34#define DART_DYNAMICS_FREEJOINT_HPP_
35
36#include <string>
37
38#include <Eigen/Dense>
39
41
42namespace dart {
43namespace dynamics {
44
46class FreeJoint : public GenericJoint<math::SE3Space>
47{
48public:
49
50 friend class Skeleton;
51
53
62
63 FreeJoint(const FreeJoint&) = delete;
64
66 virtual ~FreeJoint();
67
70
71 // Documentation inherited
72 const std::string& getType() const override;
73
75 static const std::string& getStaticType();
76
77 // Documentation inherited
78 bool isCyclic(std::size_t _index) const override;
79
86 static Eigen::Vector6d convertToPositions(const Eigen::Isometry3d& _tf);
87
89 static Eigen::Isometry3d convertToTransform(const Eigen::Vector6d& _positions);
90
94 static void setTransform(Joint* joint,
95 const Eigen::Isometry3d& tf,
96 const Frame* withRespectTo = Frame::World());
97
101 static void setTransform(BodyNode* bodyNode,
102 const Eigen::Isometry3d& tf,
103 const Frame* withRespectTo = Frame::World());
104
109 static void setTransform(Skeleton* skeleton,
110 const Eigen::Isometry3d& tf,
111 const Frame* withRespectTo = Frame::World(),
112 bool applyToAllRootBodies = true);
113
140 void setSpatialMotion(
141 const Eigen::Isometry3d* newTransform,
142 const Frame* withRespectTo,
143 const Eigen::Vector6d* newSpatialVelocity,
144 const Frame* velRelativeTo,
145 const Frame* velInCoordinatesOf,
146 const Eigen::Vector6d* newSpatialAcceleration,
147 const Frame* accRelativeTo,
148 const Frame* accInCoordinatesOf);
149
152 void setRelativeTransform(const Eigen::Isometry3d& newTransform);
153
157 void setTransform(const Eigen::Isometry3d& newTransform,
158 const Frame* withRespectTo = Frame::World());
159
165 void setRelativeSpatialVelocity(const Eigen::Vector6d& newSpatialVelocity);
166
172 void setRelativeSpatialVelocity(const Eigen::Vector6d& newSpatialVelocity,
173 const Frame* inCoordinatesOf);
174
181 void setSpatialVelocity(const Eigen::Vector6d& newSpatialVelocity,
182 const Frame* relativeTo,
183 const Frame* inCoordinatesOf);
184
193 void setLinearVelocity(const Eigen::Vector3d& newLinearVelocity,
194 const Frame* relativeTo = Frame::World(),
195 const Frame* inCoordinatesOf = Frame::World());
196
205 void setAngularVelocity(const Eigen::Vector3d& newAngularVelocity,
206 const Frame* relativeTo = Frame::World(),
207 const Frame* inCoordinatesOf = Frame::World());
208
215 const Eigen::Vector6d& newSpatialAcceleration);
216
224 const Eigen::Vector6d& newSpatialAcceleration,
225 const Frame* inCoordinatesOf);
226
234 void setSpatialAcceleration(const Eigen::Vector6d& newSpatialAcceleration,
235 const Frame* relativeTo,
236 const Frame* inCoordinatesOf);
237
246 void setLinearAcceleration(const Eigen::Vector3d& newLinearAcceleration,
247 const Frame* relativeTo = Frame::World(),
248 const Frame* inCoordinatesOf = Frame::World());
249
258 void setAngularAcceleration(const Eigen::Vector3d& newAngularAcceleration,
259 const Frame* relativeTo = Frame::World(),
260 const Frame* inCoordinatesOf = Frame::World());
261
262 // Documentation inherited
264 const Eigen::Vector6d& _positions) const override;
265
266 // Documentation inherited
268 const Eigen::Vector6d& _q2, const Eigen::Vector6d& _q1) const override;
269
270protected:
271
274
275 // Documentation inherited
276 Joint* clone() const override;
277
279
280 // Documentation inherited
281 void integratePositions(double _dt) override;
282
283 // Documentation inherited
284 void updateDegreeOfFreedomNames() override;
285
286 // Documentation inherited
287 void updateRelativeTransform() const override;
288
289 // Documentation inherited
290 void updateRelativeJacobian(bool _mandatory = true) const override;
291
292 // Documentation inherited
293 void updateRelativeJacobianTimeDeriv() const override;
294
295protected:
296
298 const Eigen::Isometry3d& getQ() const;
299
303 mutable Eigen::Isometry3d mQ;
304
305public:
306 // To get byte-aligned Eigen vectors
307 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
308};
309
310} // namespace dynamics
311} // namespace dart
312
313#endif // DART_DYNAMICS_FREEJOINT_HPP_
#define DART_DEFINE_ALIGNED_SHARED_OBJECT_CREATOR(class_name)
Definition Memory.hpp:148
BodyPropPtr properties
Definition SdfParser.cpp:80
BodyNode class represents a single node of the skeleton.
Definition BodyNode.hpp:78
The Frame class serves as the backbone of DART's kinematic tree structure.
Definition Frame.hpp:57
static Frame * World()
Definition Frame.cpp:72
class FreeJoint
Definition FreeJoint.hpp:47
void setRelativeTransform(const Eigen::Isometry3d &newTransform)
Set the transform of the child BodyNode relative to the parent BodyNode.
Definition FreeJoint.cpp:163
Eigen::Vector6d getPositionDifferencesStatic(const Eigen::Vector6d &_q2, const Eigen::Vector6d &_q1) const override
Definition FreeJoint.cpp:513
FreeJoint(const FreeJoint &)=delete
void setLinearAcceleration(const Eigen::Vector3d &newLinearAcceleration, const Frame *relativeTo=Frame::World(), const Frame *inCoordinatesOf=Frame::World())
Set the linear portion of classical acceleration of the child BodyNode relative to an arbitrary Frame...
Definition FreeJoint.cpp:428
Properties getFreeJointProperties() const
Get the Properties of this FreeJoint.
Definition FreeJoint.cpp:58
void updateDegreeOfFreedomNames() override
Definition FreeJoint.cpp:572
static const std::string & getStaticType()
Get joint type for this class.
Definition FreeJoint.cpp:549
void updateRelativeJacobian(bool _mandatory=true) const override
Definition FreeJoint.cpp:600
void updateRelativeTransform() const override
Definition FreeJoint.cpp:589
void setRelativeSpatialVelocity(const Eigen::Vector6d &newSpatialVelocity)
Set the spatial velocity of the child BodyNode relative to the parent BodyNode.
Definition FreeJoint.cpp:183
void setLinearVelocity(const Eigen::Vector3d &newLinearVelocity, const Frame *relativeTo=Frame::World(), const Frame *inCoordinatesOf=Frame::World())
Set the linear portion of classical velocity of the child BodyNode relative to an arbitrary Frame.
Definition FreeJoint.cpp:262
void setRelativeSpatialAcceleration(const Eigen::Vector6d &newSpatialAcceleration)
Set the spatial acceleration of the child BodyNode relative to the parent BodyNode.
Definition FreeJoint.cpp:334
void setSpatialMotion(const Eigen::Isometry3d *newTransform, const Frame *withRespectTo, const Eigen::Vector6d *newSpatialVelocity, const Frame *velRelativeTo, const Frame *velInCoordinatesOf, const Eigen::Vector6d *newSpatialAcceleration, const Frame *accRelativeTo, const Frame *accInCoordinatesOf)
Set the transform, spatial velocity, and spatial acceleration of the child BodyNode relative to an ar...
Definition FreeJoint.cpp:139
void setAngularAcceleration(const Eigen::Vector3d &newAngularAcceleration, const Frame *relativeTo=Frame::World(), const Frame *inCoordinatesOf=Frame::World())
Set the angular portion of classical acceleration of the child BodyNode relative to an arbitrary Fram...
Definition FreeJoint.cpp:468
void updateRelativeJacobianTimeDeriv() const override
Definition FreeJoint.cpp:607
void setSpatialAcceleration(const Eigen::Vector6d &newSpatialAcceleration, const Frame *relativeTo, const Frame *inCoordinatesOf)
Set the spatial acceleration of the child BodyNode relative to an arbitrary Frame.
Definition FreeJoint.cpp:364
void integratePositions(double _dt) override
Definition FreeJoint.cpp:563
Joint * clone() const override
Definition FreeJoint.cpp:537
const std::string & getType() const override
Definition FreeJoint.cpp:543
static void setTransform(Joint *joint, const Eigen::Isometry3d &tf, const Frame *withRespectTo=Frame::World())
If the given joint is a FreeJoint, then set the transform of the given Joint's child BodyNode so that...
Definition FreeJoint.cpp:83
void setAngularVelocity(const Eigen::Vector3d &newAngularVelocity, const Frame *relativeTo=Frame::World(), const Frame *inCoordinatesOf=Frame::World())
Set the angular portion of classical velocity of the child BodyNode relative to an arbitrary Frame.
Definition FreeJoint.cpp:298
static Eigen::Isometry3d convertToTransform(const Eigen::Vector6d &_positions)
Convert a FreeJoint-style 6D vector into a transform.
Definition FreeJoint.cpp:73
static Eigen::Vector6d convertToPositions(const Eigen::Isometry3d &_tf)
Convert a transform into a 6D vector that can be used to set the positions of a FreeJoint.
Definition FreeJoint.cpp:64
const Eigen::Isometry3d & getQ() const
Access mQ, which is an auto-updating variable.
Definition FreeJoint.cpp:613
Eigen::Isometry3d mQ
Transformation matrix dependent on generalized coordinates.
Definition FreeJoint.hpp:303
virtual ~FreeJoint()
Destructor.
Definition FreeJoint.cpp:52
void setSpatialVelocity(const Eigen::Vector6d &newSpatialVelocity, const Frame *relativeTo, const Frame *inCoordinatesOf)
Set the spatial velocity of the child BodyNode relative to an arbitrary Frame.
Definition FreeJoint.cpp:209
bool isCyclic(std::size_t _index) const override
Definition FreeJoint.cpp:556
Definition GenericJoint.hpp:49
const GenericJoint< math::SE3Space >::JacobianMatrix & getRelativeJacobianStatic() const
Fixed-size version of getRelativeJacobian()
Definition GenericJoint.hpp:1588
detail::GenericJointProperties< math::SE3Space > Properties
Definition GenericJoint.hpp:64
class Joint
Definition Joint.hpp:59
class Skeleton
Definition Skeleton.hpp:59
Matrix< double, 6, 1 > Vector6d
Definition MathTypes.hpp:49
Matrix< double, 6, 6 > Matrix6d
Definition MathTypes.hpp:50
Definition BulletCollisionDetector.cpp:63
Definition FreeJoint.hpp:55