DART  6.7.3
ZeroDofJoint.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_ZERODOFJOINT_HPP_
34 #define DART_DYNAMICS_ZERODOFJOINT_HPP_
35 
36 #include <string>
37 
38 #include "dart/dynamics/Joint.hpp"
39 
40 namespace dart {
41 namespace dynamics {
42 
43 class BodyNode;
44 class Skeleton;
45 
47 class ZeroDofJoint : public Joint
48 {
49 public:
50 
52  {
53  Properties(const Joint::Properties& _properties = Joint::Properties());
54  virtual ~Properties() = default;
55  };
56 
57  ZeroDofJoint(const ZeroDofJoint&) = delete;
58 
60  virtual ~ZeroDofJoint();
61 
64 
65  //----------------------------------------------------------------------------
66  // Interface for generalized coordinates
67  //----------------------------------------------------------------------------
68 
69  // Documentation inherited
70  DegreeOfFreedom* getDof(std::size_t) override;
71 
72  // Documentation inherited
73  const DegreeOfFreedom* getDof(std::size_t) const override;
74 
75  // Documentation inherited
76  const std::string& setDofName(std::size_t, const std::string&, bool ) override;
77 
78  // Documentation inherited
79  void preserveDofName(std::size_t, bool) override;
80 
81  // Documentation inherited
82  bool isDofNamePreserved(std::size_t) const override;
83 
84  const std::string& getDofName(std::size_t) const override;
85 
86  // Documentation inherited
87  std::size_t getNumDofs() const override;
88 
89  // Documentation inherited
90  std::size_t getIndexInSkeleton(std::size_t _index) const override;
91 
92  // Documentation inherited
93  std::size_t getIndexInTree(std::size_t _index) const override;
94 
95  //----------------------------------------------------------------------------
96  // Command
97  //----------------------------------------------------------------------------
98 
99  // Documentation inherited
100  void setCommand(std::size_t _index, double _command) override;
101 
102  // Documentation inherited
103  double getCommand(std::size_t _index) const override;
104 
105  // Documentation inherited
106  void setCommands(const Eigen::VectorXd& _commands) override;
107 
108  // Documentation inherited
109  Eigen::VectorXd getCommands() const override;
110 
111  // Documentation inherited
112  void resetCommands() override;
113 
114  //----------------------------------------------------------------------------
115  // Position
116  //----------------------------------------------------------------------------
117 
118  // Documentation inherited
119  void setPosition(std::size_t, double) override;
120 
121  // Documentation inherited
122  double getPosition(std::size_t _index) const override;
123 
124  // Documentation inherited
125  void setPositions(const Eigen::VectorXd& _positions) override;
126 
127  // Documentation inherited
128  Eigen::VectorXd getPositions() const override;
129 
130  // Documentation inherited
131  void setPositionLowerLimit(std::size_t _index, double _position) override;
132 
133  // Documentation inherited
134  double getPositionLowerLimit(std::size_t _index) const override;
135 
136  // Documentation inherited
137  void setPositionLowerLimits(const Eigen::VectorXd& lowerLimits) override;
138 
139  // Documentation inherited
140  Eigen::VectorXd getPositionLowerLimits() const override;
141 
142  // Documentation inherited
143  void setPositionUpperLimit(std::size_t index, double position) override;
144 
145  // Documentation inherited
146  double getPositionUpperLimit(std::size_t index) const override;
147 
148  // Documentation inherited
149  void setPositionUpperLimits(const Eigen::VectorXd& upperLimits) override;
150 
151  // Documentation inherited
152  Eigen::VectorXd getPositionUpperLimits() const override;
153 
154  // Documentation inherited
155  bool hasPositionLimit(std::size_t _index) const override;
156 
157  // Documentation inherited
158  void resetPosition(std::size_t _index) override;
159 
160  // Documentation inherited
161  void resetPositions() override;
162 
163  // Documentation inherited
164  void setInitialPosition(std::size_t _index, double _initial) override;
165 
166  // Documentation inherited
167  double getInitialPosition(std::size_t _index) const override;
168 
169  // Documentation inherited
170  void setInitialPositions(const Eigen::VectorXd& _initial) override;
171 
172  // Documentation inherited
173  Eigen::VectorXd getInitialPositions() const override;
174 
175  //----------------------------------------------------------------------------
176  // Velocity
177  //----------------------------------------------------------------------------
178 
179  // Documentation inherited
180  void setVelocity(std::size_t _index, double _velocity) override;
181 
182  // Documentation inherited
183  double getVelocity(std::size_t _index) const override;
184 
185  // Documentation inherited
186  void setVelocities(const Eigen::VectorXd& _velocities) override;
187 
188  // Documentation inherited
189  Eigen::VectorXd getVelocities() const override;
190 
191  // Documentation inherited
192  void setVelocityLowerLimit(std::size_t _index, double _velocity) override;
193 
194  // Documentation inherited
195  double getVelocityLowerLimit(std::size_t _index) const override;
196 
197  // Documentation inherited
198  void setVelocityLowerLimits(const Eigen::VectorXd& lowerLimits) override;
199 
200  // Documentation inherited
201  Eigen::VectorXd getVelocityLowerLimits() const override;
202 
203  // Documentation inherited
204  void setVelocityUpperLimit(std::size_t _index, double _velocity) override;
205 
206  // Documentation inherited
207  double getVelocityUpperLimit(std::size_t _index) const override;
208 
209  // Documentation inherited
210  void setVelocityUpperLimits(const Eigen::VectorXd& upperLimits) override;
211 
212  // Documentation inherited
213  Eigen::VectorXd getVelocityUpperLimits() const override;
214 
215  // Documentation inherited
216  void resetVelocity(std::size_t _index) override;
217 
218  // Documentation inherited
219  void resetVelocities() override;
220 
221  // Documentation inherited
222  void setInitialVelocity(std::size_t _index, double _initial) override;
223 
224  // Documentation inherited
225  double getInitialVelocity(std::size_t _index) const override;
226 
227  // Documentation inherited
228  void setInitialVelocities(const Eigen::VectorXd& _initial) override;
229 
230  // Documentation inherited
231  Eigen::VectorXd getInitialVelocities() const override;
232 
233  //----------------------------------------------------------------------------
234  // Acceleration
235  //----------------------------------------------------------------------------
236 
237  // Documentation inherited
238  void setAcceleration(std::size_t _index, double _acceleration) override;
239 
240  // Documentation inherited
241  double getAcceleration(std::size_t _index) const override;
242 
243  // Documentation inherited
244  void setAccelerations(const Eigen::VectorXd& _accelerations) override;
245 
246  // Documentation inherited
247  Eigen::VectorXd getAccelerations() const override;
248 
249  // Documentation inherited
250  void resetAccelerations() override;
251 
252  // Documentation inherited
253  void setAccelerationLowerLimit(std::size_t _index, double _acceleration) override;
254 
255  // Documentation inherited
256  double getAccelerationLowerLimit(std::size_t _index) const override;
257 
258  // Documentation inherited
259  void setAccelerationLowerLimits(const Eigen::VectorXd& lowerLimits) override;
260 
261  // Documentation inherited
262  Eigen::VectorXd getAccelerationLowerLimits() const override;
263 
264  // Documentation inherited
265  void setAccelerationUpperLimit(std::size_t _index, double _acceleration) override;
266 
267  // Documentation inherited
268  double getAccelerationUpperLimit(std::size_t _index) const override;
269 
270  // Documentation inherited
271  void setAccelerationUpperLimits(const Eigen::VectorXd& upperLimits) override;
272 
273  // Documentation inherited
274  Eigen::VectorXd getAccelerationUpperLimits() const override;
275 
276  //----------------------------------------------------------------------------
277  // Force
278  //----------------------------------------------------------------------------
279 
280  // Documentation inherited
281  void setForce(std::size_t _index, double _force) override;
282 
283  // Documentation inherited
284  double getForce(std::size_t _index) const override;
285 
286  // Documentation inherited
287  void setForces(const Eigen::VectorXd& _forces) override;
288 
289  // Documentation inherited
290  Eigen::VectorXd getForces() const override;
291 
292  // Documentation inherited
293  void resetForces() override;
294 
295  // Documentation inherited
296  void setForceLowerLimit(std::size_t _index, double _force) override;
297 
298  // Documentation inherited
299  double getForceLowerLimit(std::size_t _index) const override;
300 
301  // Documentation inherited
302  void setForceLowerLimits(const Eigen::VectorXd& lowerLimits) override;
303 
304  // Documentation inherited
305  Eigen::VectorXd getForceLowerLimits() const override;
306 
307  // Documentation inherited
308  void setForceUpperLimit(std::size_t _index, double _force) override;
309 
310  // Documentation inherited
311  double getForceUpperLimit(std::size_t _index) const override;
312 
313  // Documentation inherited
314  void setForceUpperLimits(const Eigen::VectorXd& upperLimits) override;
315 
316  // Documentation inherited
317  Eigen::VectorXd getForceUpperLimits() const override;
318 
319  //----------------------------------------------------------------------------
320  // Velocity change
321  //----------------------------------------------------------------------------
322 
323  // Documentation inherited
324  void setVelocityChange(std::size_t _index, double _velocityChange) override;
325 
326  // Documentation inherited
327  double getVelocityChange(std::size_t _index) const override;
328 
329  // Documentation inherited
330  void resetVelocityChanges() override;
331 
332  //----------------------------------------------------------------------------
333  // Constraint impulse
334  //----------------------------------------------------------------------------
335 
336  // Documentation inherited
337  void setConstraintImpulse(std::size_t _index, double _impulse) override;
338 
339  // Documentation inherited
340  double getConstraintImpulse(std::size_t _index) const override;
341 
342  // Documentation inherited
343  void resetConstraintImpulses() override;
344 
345  //----------------------------------------------------------------------------
346  // Integration and finite difference
347  //----------------------------------------------------------------------------
348 
349  // Documentation inherited
350  void integratePositions(double _dt) override;
351 
352  // Documentation inherited
353  void integrateVelocities(double _dt) override;
354 
355  // Documentation inherited
356  Eigen::VectorXd getPositionDifferences(
357  const Eigen::VectorXd& _q2, const Eigen::VectorXd& _q1) const override;
358 
359  //----------------------------------------------------------------------------
361  //----------------------------------------------------------------------------
362 
363  // Documentation inherited
364  void setSpringStiffness(std::size_t _index, double _k) override;
365 
366  // Documentation inherited
367  double getSpringStiffness(std::size_t _index) const override;
368 
369  // Documentation inherited
370  void setRestPosition(std::size_t _index, double _q0) override;
371 
372  // Documentation inherited
373  double getRestPosition(std::size_t _index) const override;
374 
375  // Documentation inherited
376  void setDampingCoefficient(std::size_t _index, double _d) override;
377 
378  // Documentation inherited
379  double getDampingCoefficient(std::size_t _index) const override;
380 
381  // Documentation inherited
382  void setCoulombFriction(std::size_t _index, double _friction) override;
383 
384  // Documentation inherited
385  double getCoulombFriction(std::size_t _index) const override;
386 
388 
389  //----------------------------------------------------------------------------
390 
391  // Documentation inherited
392  double computePotentialEnergy() const override;
393 
394  // Documentation inherited
395  Eigen::Vector6d getBodyConstraintWrench() const override;
396 
397 protected:
398 
400  ZeroDofJoint();
401 
402  // Documentation inherited
403  void registerDofs() override;
404 
405  // Documentation inherited
406  void updateDegreeOfFreedomNames() override;
407 
408  //----------------------------------------------------------------------------
410  //----------------------------------------------------------------------------
411 
412  // Documentation inherited
413  const math::Jacobian getRelativeJacobian() const override;
414 
415  // Documentation inherited
417  const Eigen::VectorXd& _positions) const override;
418 
419  // Documentation inherited
420  const math::Jacobian getRelativeJacobianTimeDeriv() const override;
421 
422  // Documentation inherited
423  void addVelocityTo(Eigen::Vector6d& _vel) override;
424 
425  // Documentation inherited
427  Eigen::Vector6d& _partialAcceleration,
428  const Eigen::Vector6d& _childVelocity) override;
429 
430  // Documentation inherited
431  void addAccelerationTo(Eigen::Vector6d& _acc) override;
432 
433  // Documentation inherited
434  void addVelocityChangeTo(Eigen::Vector6d& _velocityChange) override;
435 
436  // Documentation inherited
438  Eigen::Matrix6d& _parentArtInertia,
439  const Eigen::Matrix6d& _childArtInertia) override;
440 
441  // Documentation inherited
443  Eigen::Matrix6d& _parentArtInertia,
444  const Eigen::Matrix6d& _childArtInertia) override;
445 
446  // Documentation inherited
448  const Eigen::Matrix6d& _artInertia) override;
449 
450  // Documentation inherited
452  const Eigen::Matrix6d& _artInertia,
453  double _timeStep) override;
454 
455  // Documentation inherited
456  void addChildBiasForceTo(
457  Eigen::Vector6d& _parentBiasForce,
458  const Eigen::Matrix6d& _childArtInertia,
459  const Eigen::Vector6d& _childBiasForce,
460  const Eigen::Vector6d& _childPartialAcc) override;
461 
462  // Documentation inherited
464  Eigen::Vector6d& _parentBiasImpulse,
465  const Eigen::Matrix6d& _childArtInertia,
466  const Eigen::Vector6d& _childBiasImpulse) override;
467 
468  // Documentation inherited
469  void updateTotalForce(const Eigen::Vector6d& _bodyForce,
470  double _timeStep) override;
471 
472  // Documentation inherited
473  void updateTotalImpulse(
474  const Eigen::Vector6d& _bodyImpulse) override;
475 
476  // Documentation inherited
477  void resetTotalImpulses() override;
478 
479  // Documentation inherited
480  void updateAcceleration(
481  const Eigen::Matrix6d& _artInertia,
482  const Eigen::Vector6d& _spatialAcc) override;
483 
484  // Documentation inherited
486  const Eigen::Matrix6d& _artInertia,
487  const Eigen::Vector6d& _velocityChange) override;
488 
489  // Documentation inherited
490  void updateForceID(const Eigen::Vector6d& _bodyForce,
491  double _timeStep,
492  bool _withDampingForces,
493  bool _withSpringForces) override;
494 
495  // Documentation inherited
496  void updateForceFD(const Eigen::Vector6d& _bodyForce,
497  double _timeStep,
498  bool _withDampingForces,
499  bool _withSpringForces) override;
500 
501  // Documentation inherited
502  void updateImpulseID(const Eigen::Vector6d& _bodyImpulse) override;
503 
504  // Documentation inherited
505  void updateImpulseFD(const Eigen::Vector6d& _bodyImpulse) override;
506 
507  // Documentation inherited
508  void updateConstrainedTerms(double _timeStep) override;
509 
511 
512  //----------------------------------------------------------------------------
514  //----------------------------------------------------------------------------
515 
518  Eigen::Vector6d& _parentBiasForce,
519  const Eigen::Matrix6d& _childArtInertia,
520  const Eigen::Vector6d& _childBiasForce) override;
521 
524  Eigen::Vector6d& _parentBiasForce,
525  const Eigen::Matrix6d& _childArtInertia,
526  const Eigen::Vector6d& _childBiasForce) override;
527 
530  const Eigen::Vector6d& _bodyForce) override;
531 
532  // Documentation inherited
533  void getInvMassMatrixSegment(Eigen::MatrixXd& _invMassMat,
534  const std::size_t _col,
535  const Eigen::Matrix6d& _artInertia,
536  const Eigen::Vector6d& _spatialAcc) override;
537 
538  // Documentation inherited
539  void getInvAugMassMatrixSegment(Eigen::MatrixXd& _invMassMat,
540  const std::size_t _col,
541  const Eigen::Matrix6d& _artInertia,
542  const Eigen::Vector6d& _spatialAcc) override;
543 
544  // Documentation inherited
545  void addInvMassMatrixSegmentTo(Eigen::Vector6d& _acc) override;
546 
547  // Documentation inherited
548  Eigen::VectorXd getSpatialToGeneralized(
549  const Eigen::Vector6d& _spatial) override;
550 
552 
553 private:
554 
556  const std::string emptyString;
557 };
558 
559 } // namespace dynamics
560 } // namespace dart
561 
562 #endif // DART_DYNAMICS_ZERODOFJOINT_HPP_
std::size_t index
Definition: SkelParser.cpp:1617
Eigen::VectorXd position
Definition: SkelParser.cpp:107
DegreeOfFreedom class is a proxy class for accessing single degrees of freedom (aka generalized coord...
Definition: DegreeOfFreedom.hpp:53
class Joint
Definition: Joint.hpp:59
class ZeroDofJoint
Definition: ZeroDofJoint.hpp:48
const std::string & setDofName(std::size_t, const std::string &, bool) override
Alternative to DegreeOfFreedom::setName()
Definition: ZeroDofJoint.cpp:81
double getPositionLowerLimit(std::size_t _index) const override
Get lower limit for position.
Definition: ZeroDofJoint.cpp:198
void addVelocityChangeTo(Eigen::Vector6d &_velocityChange) override
Add joint velocity change to _velocityChange.
Definition: ZeroDofJoint.cpp:714
void addChildBiasForceForInvAugMassMatrix(Eigen::Vector6d &_parentBiasForce, const Eigen::Matrix6d &_childArtInertia, const Eigen::Vector6d &_childBiasForce) override
Add child's bias force to parent's one.
Definition: ZeroDofJoint.cpp:873
Eigen::VectorXd getVelocities() const override
Get the velocities of all generalized coordinates in this Joint.
Definition: ZeroDofJoint.cpp:303
const std::string & getDofName(std::size_t) const override
Alternative to DegreeOfFreedom::getName()
Definition: ZeroDofJoint.cpp:99
void setForceLowerLimits(const Eigen::VectorXd &lowerLimits) override
Set the force upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:517
void updateImpulseFD(const Eigen::Vector6d &_bodyImpulse) override
Update joint impulses for forward dynamics.
Definition: ZeroDofJoint.cpp:852
std::size_t getIndexInTree(std::size_t _index) const override
Get a unique index in the kinematic tree of a generalized coordinate in this Joint.
Definition: ZeroDofJoint.cpp:121
void setAccelerationUpperLimit(std::size_t _index, double _acceleration) override
Set upper limit for acceleration.
Definition: ZeroDofJoint.cpp:450
bool isDofNamePreserved(std::size_t) const override
Alternative to DegreeOfFreedom::isNamePreserved()
Definition: ZeroDofJoint.cpp:93
double getConstraintImpulse(std::size_t _index) const override
Get a single constraint impulse.
Definition: ZeroDofJoint.cpp:578
void setCommands(const Eigen::VectorXd &_commands) override
Set all commands for this Joint.
Definition: ZeroDofJoint.cpp:146
void setCoulombFriction(std::size_t _index, double _friction) override
Set joint Coulomb friction froce.
Definition: ZeroDofJoint.cpp:646
void addInvMassMatrixSegmentTo(Eigen::Vector6d &_acc) override
Definition: ZeroDofJoint.cpp:909
void addChildBiasForceForInvMassMatrix(Eigen::Vector6d &_parentBiasForce, const Eigen::Matrix6d &_childArtInertia, const Eigen::Vector6d &_childBiasForce) override
Add child's bias force to parent's one.
Definition: ZeroDofJoint.cpp:864
Eigen::VectorXd getPositionLowerLimits() const override
Get the position lower limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:211
double getCommand(std::size_t _index) const override
Get a single command.
Definition: ZeroDofJoint.cpp:137
double getSpringStiffness(std::size_t _index) const override
Get stiffness of joint spring force.
Definition: ZeroDofJoint.cpp:616
void setVelocityChange(std::size_t _index, double _velocityChange) override
Set a single velocity change.
Definition: ZeroDofJoint.cpp:553
double getVelocity(std::size_t _index) const override
Get the velocity of a single generalized coordinate.
Definition: ZeroDofJoint.cpp:291
void setPartialAccelerationTo(Eigen::Vector6d &_partialAcceleration, const Eigen::Vector6d &_childVelocity) override
Set joint partial acceleration to _partialAcceleration.
Definition: ZeroDofJoint.cpp:720
void setConstraintImpulse(std::size_t _index, double _impulse) override
Set a single constraint impulse.
Definition: ZeroDofJoint.cpp:572
Eigen::VectorXd getPositions() const override
Get the positions of all generalized coordinates in this Joint.
Definition: ZeroDofJoint.cpp:185
const math::Jacobian getRelativeJacobian() const override
Get spatial Jacobian of the child BodyNode relative to the parent BodyNode expressed in the child Bod...
Definition: ZeroDofJoint.cpp:689
void addChildArtInertiaTo(Eigen::Matrix6d &_parentArtInertia, const Eigen::Matrix6d &_childArtInertia) override
Add child's articulated inertia to parent's one.
Definition: ZeroDofJoint.cpp:734
void setInitialPositions(const Eigen::VectorXd &_initial) override
Change the positions that resetPositions() will give to this Joint's coordinates.
Definition: ZeroDofJoint.cpp:273
void preserveDofName(std::size_t, bool) override
Alternative to DegreeOfFreedom::preserveName()
Definition: ZeroDofJoint.cpp:87
void addChildBiasImpulseTo(Eigen::Vector6d &_parentBiasImpulse, const Eigen::Matrix6d &_childArtInertia, const Eigen::Vector6d &_childBiasImpulse) override
Add child's bias impulse to parent's one.
Definition: ZeroDofJoint.cpp:783
double getVelocityLowerLimit(std::size_t _index) const override
Get lower limit for velocity.
Definition: ZeroDofJoint.cpp:316
double computePotentialEnergy() const override
Compute and return the potential energy.
Definition: ZeroDofJoint.cpp:658
void setPositionUpperLimits(const Eigen::VectorXd &upperLimits) override
Set the position upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:230
void addChildBiasForceTo(Eigen::Vector6d &_parentBiasForce, const Eigen::Matrix6d &_childArtInertia, const Eigen::Vector6d &_childBiasForce, const Eigen::Vector6d &_childPartialAcc) override
Add child's bias force to parent's one.
Definition: ZeroDofJoint.cpp:770
void setForceUpperLimits(const Eigen::VectorXd &upperLimits) override
Set the force upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:541
double getRestPosition(std::size_t _index) const override
Get rest position of spring force.
Definition: ZeroDofJoint.cpp:628
double getForceLowerLimit(std::size_t _index) const override
Get lower limit for force.
Definition: ZeroDofJoint.cpp:511
void integrateVelocities(double _dt) override
Integrate velocities using Euler method.
Definition: ZeroDofJoint.cpp:596
Eigen::VectorXd getForceUpperLimits() const override
Get the force upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:547
Eigen::VectorXd getAccelerationLowerLimits() const override
Get the acceleration upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:444
void setVelocityUpperLimit(std::size_t _index, double _velocity) override
Set upper limit for velocity.
Definition: ZeroDofJoint.cpp:334
void updateForceID(const Eigen::Vector6d &_bodyForce, double _timeStep, bool _withDampingForces, bool _withSpringForces) override
Update joint force for inverse dynamics.
Definition: ZeroDofJoint.cpp:828
void updateImpulseID(const Eigen::Vector6d &_bodyImpulse) override
Update joint impulses for inverse dynamics.
Definition: ZeroDofJoint.cpp:846
void addChildArtInertiaImplicitTo(Eigen::Matrix6d &_parentArtInertia, const Eigen::Matrix6d &_childArtInertia) override
Add child's articulated inertia to parent's one. Forward dynamics routine.
Definition: ZeroDofJoint.cpp:745
Properties getZeroDofJointProperties() const
Get the Properties of this ZeroDofJoint.
Definition: ZeroDofJoint.cpp:57
ZeroDofJoint()
Constructor called by inheriting classes.
Definition: ZeroDofJoint.cpp:664
void setAccelerationUpperLimits(const Eigen::VectorXd &upperLimits) override
Set the acceleration upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:463
void addAccelerationTo(Eigen::Vector6d &_acc) override
Add joint acceleration to _acc.
Definition: ZeroDofJoint.cpp:728
void resetCommands() override
Set all the commands for this Joint to zero.
Definition: ZeroDofJoint.cpp:158
Eigen::VectorXd getForceLowerLimits() const override
Get the force upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:523
std::size_t getNumDofs() const override
Get number of generalized coordinates.
Definition: ZeroDofJoint.cpp:105
Eigen::Vector6d getBodyConstraintWrench() const override
Get constraint wrench expressed in body node frame.
Definition: ZeroDofJoint.cpp:682
const std::string emptyString
Used by getDofName()
Definition: ZeroDofJoint.hpp:556
void setInitialVelocities(const Eigen::VectorXd &_initial) override
Change the velocities that resetVelocities() will give to this Joint's coordinates.
Definition: ZeroDofJoint.cpp:383
void resetConstraintImpulses() override
Set zero all the constraint impulses.
Definition: ZeroDofJoint.cpp:584
void getInvAugMassMatrixSegment(Eigen::MatrixXd &_invMassMat, const std::size_t _col, const Eigen::Matrix6d &_artInertia, const Eigen::Vector6d &_spatialAcc) override
Definition: ZeroDofJoint.cpp:899
void setForce(std::size_t _index, double _force) override
Set the force of a single generalized coordinate.
Definition: ZeroDofJoint.cpp:475
double getAccelerationLowerLimit(std::size_t _index) const override
Get lower limit for acceleration.
Definition: ZeroDofJoint.cpp:432
void setPosition(std::size_t, double) override
Set the position of a single generalized coordinate.
Definition: ZeroDofJoint.cpp:164
double getForce(std::size_t _index) const override
Get the force of a single generalized coordinate.
Definition: ZeroDofJoint.cpp:481
void setVelocityLowerLimit(std::size_t _index, double _velocity) override
Set lower limit for velocity.
Definition: ZeroDofJoint.cpp:309
void resetVelocityChanges() override
Set zero all the velocity change.
Definition: ZeroDofJoint.cpp:566
void registerDofs() override
Called by the Skeleton class.
Definition: ZeroDofJoint.cpp:670
Eigen::VectorXd getSpatialToGeneralized(const Eigen::Vector6d &_spatial) override
Definition: ZeroDofJoint.cpp:915
double getVelocityChange(std::size_t _index) const override
Get a single velocity change.
Definition: ZeroDofJoint.cpp:560
void setSpringStiffness(std::size_t _index, double _k) override
Set stiffness of joint spring force.
Definition: ZeroDofJoint.cpp:610
void resetPosition(std::size_t _index) override
Set the position of this generalized coordinate to its initial position.
Definition: ZeroDofJoint.cpp:249
void setCommand(std::size_t _index, double _command) override
Set a single command.
Definition: ZeroDofJoint.cpp:131
Eigen::VectorXd getAccelerations() const override
Get the accelerations of all generalized coordinates in this Joint.
Definition: ZeroDofJoint.cpp:413
void addVelocityTo(Eigen::Vector6d &_vel) override
Add joint velocity to _vel.
Definition: ZeroDofJoint.cpp:708
double getInitialVelocity(std::size_t _index) const override
Get the velocity that resetVelocity() will give to this coordinate.
Definition: ZeroDofJoint.cpp:377
void setRestPosition(std::size_t _index, double _q0) override
Set rest position of spring force.
Definition: ZeroDofJoint.cpp:622
Eigen::VectorXd getCommands() const override
Get all commands for this Joint.
Definition: ZeroDofJoint.cpp:152
void setAcceleration(std::size_t _index, double _acceleration) override
Set the acceleration of a single generalized coordinate.
Definition: ZeroDofJoint.cpp:395
std::size_t getIndexInSkeleton(std::size_t _index) const override
Get a unique index in skeleton of a generalized coordinate in this Joint.
Definition: ZeroDofJoint.cpp:111
void resetForces() override
Set the forces of all generalized coordinates in this Joint to zero.
Definition: ZeroDofJoint.cpp:499
void setInitialVelocity(std::size_t _index, double _initial) override
Change the velocity that resetVelocity() will give to this coordinate.
Definition: ZeroDofJoint.cpp:371
void updateVelocityChange(const Eigen::Matrix6d &_artInertia, const Eigen::Vector6d &_velocityChange) override
Update joint velocity change.
Definition: ZeroDofJoint.cpp:820
void setPositionLowerLimits(const Eigen::VectorXd &lowerLimits) override
Set the position lower limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:204
void updateForceFD(const Eigen::Vector6d &_bodyForce, double _timeStep, bool _withDampingForces, bool _withSpringForces) override
Update joint force for forward dynamics.
Definition: ZeroDofJoint.cpp:837
void resetVelocities() override
Set the velocities of all generalized coordinates in this Joint to their initial velocities.
Definition: ZeroDofJoint.cpp:365
void updateInvProjArtInertiaImplicit(const Eigen::Matrix6d &_artInertia, double _timeStep) override
Forward dynamics routine.
Definition: ZeroDofJoint.cpp:762
void resetPositions() override
Set the positions of all generalized coordinates in this Joint to their initial positions.
Definition: ZeroDofJoint.cpp:255
void updateTotalImpulse(const Eigen::Vector6d &_bodyImpulse) override
Update joint total impulse.
Definition: ZeroDofJoint.cpp:801
double getDampingCoefficient(std::size_t _index) const override
Get coefficient of joint damping (viscous friction) force.
Definition: ZeroDofJoint.cpp:640
bool hasPositionLimit(std::size_t _index) const override
Get whether the position of a generalized coordinate has limits.
Definition: ZeroDofJoint.cpp:243
void updateConstrainedTerms(double _timeStep) override
Update constrained terms for forward dynamics.
Definition: ZeroDofJoint.cpp:858
void setVelocityLowerLimits(const Eigen::VectorXd &lowerLimits) override
Set the velocity lower limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:322
double getForceUpperLimit(std::size_t _index) const override
Get upper limit for force.
Definition: ZeroDofJoint.cpp:535
void getInvMassMatrixSegment(Eigen::MatrixXd &_invMassMat, const std::size_t _col, const Eigen::Matrix6d &_artInertia, const Eigen::Vector6d &_spatialAcc) override
Definition: ZeroDofJoint.cpp:889
Eigen::VectorXd getAccelerationUpperLimits() const override
Get the acceleration upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:469
Eigen::VectorXd getForces() const override
Get the forces of all generalized coordinates in this Joint.
Definition: ZeroDofJoint.cpp:493
void updateTotalForce(const Eigen::Vector6d &_bodyForce, double _timeStep) override
Update joint total force.
Definition: ZeroDofJoint.cpp:794
void setInitialPosition(std::size_t _index, double _initial) override
Change the position that resetPositions() will give to this coordinate.
Definition: ZeroDofJoint.cpp:261
DegreeOfFreedom * getDof(std::size_t) override
Get an object to access the _index-th degree of freedom (generalized coordinate) of this Joint.
Definition: ZeroDofJoint.cpp:63
double getPositionUpperLimit(std::size_t index) const override
Get upper limit for position.
Definition: ZeroDofJoint.cpp:224
void setDampingCoefficient(std::size_t _index, double _d) override
Set coefficient of joint damping (viscous friction) force.
Definition: ZeroDofJoint.cpp:634
void setForceLowerLimit(std::size_t _index, double _force) override
Set lower limit for force.
Definition: ZeroDofJoint.cpp:505
Eigen::VectorXd getVelocityUpperLimits() const override
Get the velocity upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:353
void setVelocities(const Eigen::VectorXd &_velocities) override
Set the velocities of all generalized coordinates in this Joint.
Definition: ZeroDofJoint.cpp:297
Eigen::VectorXd getPositionUpperLimits() const override
Get the position upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:237
void updateDegreeOfFreedomNames() override
Update the names of the joint's degrees of freedom.
Definition: ZeroDofJoint.cpp:676
double getVelocityUpperLimit(std::size_t _index) const override
Get upper limit for velocity.
Definition: ZeroDofJoint.cpp:341
double getInitialPosition(std::size_t _index) const override
Get the position that resetPosition() will give to this coordinate.
Definition: ZeroDofJoint.cpp:267
void updateTotalForceForInvMassMatrix(const Eigen::Vector6d &_bodyForce) override
Definition: ZeroDofJoint.cpp:882
double getCoulombFriction(std::size_t _index) const override
Get joint Coulomb friction froce.
Definition: ZeroDofJoint.cpp:652
void updateInvProjArtInertia(const Eigen::Matrix6d &_artInertia) override
Update inverse of projected articulated body inertia.
Definition: ZeroDofJoint.cpp:755
void setVelocityUpperLimits(const Eigen::VectorXd &upperLimits) override
Set the velocity upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:347
void resetTotalImpulses() override
Set total impulses to zero.
Definition: ZeroDofJoint.cpp:807
void resetVelocity(std::size_t _index) override
Set the velocity of a generalized coordinate in this Joint to its initial velocity.
Definition: ZeroDofJoint.cpp:359
void setAccelerationLowerLimit(std::size_t _index, double _acceleration) override
Set lower limit for acceleration.
Definition: ZeroDofJoint.cpp:425
Eigen::VectorXd getInitialPositions() const override
Get the positions that resetPositions() will give to this Joint's coordinates.
Definition: ZeroDofJoint.cpp:279
void integratePositions(double _dt) override
Integrate positions using Euler method.
Definition: ZeroDofJoint.cpp:590
void setVelocity(std::size_t _index, double _velocity) override
Set the velocity of a single generalized coordinate.
Definition: ZeroDofJoint.cpp:285
ZeroDofJoint(const ZeroDofJoint &)=delete
void resetAccelerations() override
Set the accelerations of all generalized coordinates in this Joint to zero.
Definition: ZeroDofJoint.cpp:419
virtual ~ZeroDofJoint()
Destructor.
Definition: ZeroDofJoint.cpp:51
void setPositions(const Eigen::VectorXd &_positions) override
Set the positions of all generalized coordinates in this Joint.
Definition: ZeroDofJoint.cpp:179
void setForceUpperLimit(std::size_t _index, double _force) override
Set upper limit for force.
Definition: ZeroDofJoint.cpp:529
double getAcceleration(std::size_t _index) const override
Get the acceleration of a single generalized coordinate.
Definition: ZeroDofJoint.cpp:401
double getPosition(std::size_t _index) const override
Get the position of a single generalized coordinate.
Definition: ZeroDofJoint.cpp:170
Eigen::VectorXd getVelocityLowerLimits() const override
Get the velocity lower limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:328
const math::Jacobian getRelativeJacobianTimeDeriv() const override
Get time derivative of spatial Jacobian of the child BodyNode relative to the parent BodyNode express...
Definition: ZeroDofJoint.cpp:702
Eigen::VectorXd getInitialVelocities() const override
Get the velocities that resetVelocities() will give to this Joint's coordinates.
Definition: ZeroDofJoint.cpp:389
Eigen::VectorXd getPositionDifferences(const Eigen::VectorXd &_q2, const Eigen::VectorXd &_q1) const override
Return the difference of two generalized coordinates which are measured in the configuration space of...
Definition: ZeroDofJoint.cpp:602
double getAccelerationUpperLimit(std::size_t _index) const override
Get upper limit for acceleration.
Definition: ZeroDofJoint.cpp:457
void setAccelerationLowerLimits(const Eigen::VectorXd &lowerLimits) override
Set the acceleration upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:438
void updateAcceleration(const Eigen::Matrix6d &_artInertia, const Eigen::Vector6d &_spatialAcc) override
Update joint acceleration.
Definition: ZeroDofJoint.cpp:813
void setPositionUpperLimit(std::size_t index, double position) override
Set upper limit for position.
Definition: ZeroDofJoint.cpp:217
void setPositionLowerLimit(std::size_t _index, double _position) override
Set lower limit for position.
Definition: ZeroDofJoint.cpp:191
void setAccelerations(const Eigen::VectorXd &_accelerations) override
Set the accelerations of all generalized coordinates in this Joint.
Definition: ZeroDofJoint.cpp:407
void setForces(const Eigen::VectorXd &_forces) override
Set the forces of all generalized coordinates in this Joint.
Definition: ZeroDofJoint.cpp:487
Matrix< double, 6, 1 > Vector6d
Definition: MathTypes.hpp:49
Matrix< double, 6, 6 > Matrix6d
Definition: MathTypes.hpp:50
Eigen::Matrix< double, 6, Eigen::Dynamic > Jacobian
Definition: MathTypes.hpp:108
Definition: BulletCollisionDetector.cpp:63
Definition: ZeroDofJoint.hpp:52
Properties(const Joint::Properties &_properties=Joint::Properties())
Definition: ZeroDofJoint.cpp:44
Definition: JointAspect.hpp:112