DART  6.7.3
GenericJoint.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_GENERICJOINT_HPP_
34 #define DART_DYNAMICS_GENERICJOINT_HPP_
35 
36 #include <string>
37 #include <array>
39 
40 namespace dart {
41 namespace dynamics {
42 
43 class DegreeOfFreedom;
44 
45 template <class ConfigSpaceT>
46 class GenericJoint :
47  public detail::GenericJointBase< GenericJoint<ConfigSpaceT>,
48  ConfigSpaceT >
49 {
50 public:
51 
52  static constexpr std::size_t NumDofs = ConfigSpaceT::NumDofs;
53 
56 
57  using Point = typename ConfigSpaceT::Point;
58  using EuclideanPoint = typename ConfigSpaceT::EuclideanPoint;
59  using Vector = typename ConfigSpaceT::Vector;
60  using JacobianMatrix = typename ConfigSpaceT::JacobianMatrix;
61  using Matrix = typename ConfigSpaceT::Matrix;
62 
65  using AspectState = typename Base::AspectState;
67 
69  typename ThisClass::Aspect, GenericJointAspect )
70 
71  GenericJoint(const ThisClass&) = delete;
72 
74  virtual ~GenericJoint();
75 
77  void setProperties(const Properties& properties);
78 
81 
83  void setAspectState(const AspectState& state);
84 
87 
90 
92  void copy(const ThisClass& otherJoint);
93 
95  void copy(const ThisClass* otherJoint);
96 
98  ThisClass& operator=(const ThisClass& other);
99 
100  //----------------------------------------------------------------------------
102  //----------------------------------------------------------------------------
103 
104  // Documentation inherited
105  DegreeOfFreedom* getDof(std::size_t index) override;
106 
107  // Documentation inherited
108  const DegreeOfFreedom* getDof(std::size_t _index) const override;
109 
110  // Documentation inherited
111  std::size_t getNumDofs() const override;
112 
113  // Documentation inherited
114  const std::string& setDofName(std::size_t index,
115  const std::string& name,
116  bool preserveName = true) override;
117 
118  // Documentation inherited
119  void preserveDofName(size_t index, bool preserve) override;
120 
121  // Documentation inherited
122  bool isDofNamePreserved(size_t index) const override;
123 
124  // Documentation inherited
125  const std::string& getDofName(size_t index) const override;
126 
127  // Documentation inherited
128  size_t getIndexInSkeleton(size_t index) const override;
129 
130  // Documentation inherited
131  size_t getIndexInTree(size_t index) const override;
132 
134 
135  //----------------------------------------------------------------------------
137  //----------------------------------------------------------------------------
138 
139  // Documentation inherited
140  void setCommand(std::size_t index, double command) override;
141 
142  // Documentation inherited
143  double getCommand(std::size_t index) const override;
144 
145  // Documentation inherited
146  void setCommands(const Eigen::VectorXd& commands) override;
147 
148  // Documentation inherited
149  Eigen::VectorXd getCommands() const override;
150 
151  // Documentation inherited
152  void resetCommands() override;
153 
155 
156  //----------------------------------------------------------------------------
158  //----------------------------------------------------------------------------
159 
160  // Documentation inherited
161  void setPosition(std::size_t index, double position) override;
162 
163  // Documentation inherited
164  double getPosition(std::size_t index) const override;
165 
166  // Documentation inherited
167  void setPositions(const Eigen::VectorXd& positions) override;
168 
169  // Documentation inherited
170  Eigen::VectorXd getPositions() const override;
171 
172  // Documentation inherited
173  void setPositionLowerLimit(std::size_t index, double position) override;
174 
175  // Documentation inherited
176  double getPositionLowerLimit(std::size_t index) const override;
177 
178  // Documentation inherited
179  void setPositionLowerLimits(const Eigen::VectorXd& lowerLimits) override;
180 
181  // Documentation inherited
182  Eigen::VectorXd getPositionLowerLimits() const override;
183 
184  // Documentation inherited
185  void setPositionUpperLimit(std::size_t index, double position) override;
186 
187  // Documentation inherited
188  double getPositionUpperLimit(std::size_t index) const override;
189 
190  // Documentation inherited
191  void setPositionUpperLimits(const Eigen::VectorXd& upperLimits) override;
192 
193  // Documentation inherited
194  Eigen::VectorXd getPositionUpperLimits() const override;
195 
196  // Documentation inherited
197  bool hasPositionLimit(std::size_t index) const override;
198 
199  // Documentation inherited
200  void resetPosition(std::size_t index) override;
201 
202  // Documentation inherited
203  void resetPositions() override;
204 
205  // Documentation inherited
206  void setInitialPosition(std::size_t index, double initial) override;
207 
208  // Documentation inherited
209  double getInitialPosition(std::size_t index) const override;
210 
211  // Documentation inherited
212  void setInitialPositions(const Eigen::VectorXd& initial) override;
213 
214  // Documentation inherited
215  Eigen::VectorXd getInitialPositions() const override;
216 
218 
219  //----------------------------------------------------------------------------
221  //----------------------------------------------------------------------------
222 
223  // Note: The fixed-size versions of these functions exist to make it easier
224  // to comply with the auto-updating design. Use these functions to avoid
225  // accessing mPosition directly, that way it is easier to ensure that the
226  // auto-updating design assumptions are being satisfied when reviewing the
227  // code.
228 
230  void setPositionsStatic(const Vector& positions);
231 
233  const Vector& getPositionsStatic() const;
234 
236  void setVelocitiesStatic(const Vector& velocities);
237 
239  const Vector& getVelocitiesStatic() const;
240 
242  void setAccelerationsStatic(const Vector& accels);
243 
245  const Vector& getAccelerationsStatic() const;
246 
248 
249  //----------------------------------------------------------------------------
251  //----------------------------------------------------------------------------
252 
253  // Documentation inherited
254  void setVelocity(std::size_t index, double velocity) override;
255 
256  // Documentation inherited
257  double getVelocity(std::size_t index) const override;
258 
259  // Documentation inherited
260  void setVelocities(const Eigen::VectorXd& velocities) override;
261 
262  // Documentation inherited
263  Eigen::VectorXd getVelocities() const override;
264 
265  // Documentation inherited
266  void setVelocityLowerLimit(std::size_t index, double velocity) override;
267 
268  // Documentation inherited
269  double getVelocityLowerLimit(std::size_t index) const override;
270 
271  // Documentation inherited
272  void setVelocityLowerLimits(const Eigen::VectorXd& lowerLimits) override;
273 
274  // Documentation inherited
275  Eigen::VectorXd getVelocityLowerLimits() const override;
276 
277  // Documentation inherited
278  void setVelocityUpperLimit(std::size_t index, double velocity) override;
279 
280  // Documentation inherited
281  double getVelocityUpperLimit(std::size_t index) const override;
282 
283  // Documentation inherited
284  void setVelocityUpperLimits(const Eigen::VectorXd& upperLimits) override;
285 
286  // Documentation inherited
287  Eigen::VectorXd getVelocityUpperLimits() const override;
288 
289  // Documentation inherited
290  void resetVelocity(std::size_t index) override;
291 
292  // Documentation inherited
293  void resetVelocities() override;
294 
295  // Documentation inherited
296  void setInitialVelocity(std::size_t index, double initial) override;
297 
298  // Documentation inherited
299  double getInitialVelocity(std::size_t index) const override;
300 
301  // Documentation inherited
302  void setInitialVelocities(const Eigen::VectorXd& initial) override;
303 
304  // Documentation inherited
305  Eigen::VectorXd getInitialVelocities() const override;
306 
308 
309  //----------------------------------------------------------------------------
311  //----------------------------------------------------------------------------
312 
313  // Documentation inherited
314  void setAcceleration(std::size_t index, double acceleration) override;
315 
316  // Documentation inherited
317  double getAcceleration(std::size_t index) const override;
318 
319  // Documentation inherited
320  void setAccelerations(const Eigen::VectorXd& accelerations) override;
321 
322  // Documentation inherited
323  Eigen::VectorXd getAccelerations() const override;
324 
325  // Documentation inherited
326  void setAccelerationLowerLimit(size_t index, double acceleration) override;
327 
328  // Documentation inherited
329  double getAccelerationLowerLimit(std::size_t index) const override;
330 
331  // Documentation inherited
332  void setAccelerationLowerLimits(const Eigen::VectorXd& lowerLimits) override;
333 
334  // Documentation inherited
335  Eigen::VectorXd getAccelerationLowerLimits() const override;
336 
337  // Documentation inherited
338  void setAccelerationUpperLimit(std::size_t index, double acceleration) override;
339 
340  // Documentation inherited
341  double getAccelerationUpperLimit(std::size_t index) const override;
342 
343  // Documentation inherited
344  void setAccelerationUpperLimits(const Eigen::VectorXd& upperLimits) override;
345 
346  // Documentation inherited
347  Eigen::VectorXd getAccelerationUpperLimits() const override;
348 
349  // Documentation inherited
350  void resetAccelerations() override;
351 
353 
354  //----------------------------------------------------------------------------
356  //----------------------------------------------------------------------------
357 
358  // Documentation inherited
359  void setForce(std::size_t index, double force) override;
360 
361  // Documentation inherited
362  double getForce(std::size_t index) const override;
363 
364  // Documentation inherited
365  void setForces(const Eigen::VectorXd& forces) override;
366 
367  // Documentation inherited
368  Eigen::VectorXd getForces() const override;
369 
370  // Documentation inherited
371  void setForceLowerLimit(size_t index, double force) override;
372 
373  // Documentation inherited
374  double getForceLowerLimit(std::size_t index) const override;
375 
376  // Documentation inherited
377  void setForceLowerLimits(const Eigen::VectorXd& lowerLimits) override;
378 
379  // Documentation inherited
380  Eigen::VectorXd getForceLowerLimits() const override;
381 
382  // Documentation inherited
383  void setForceUpperLimit(size_t index, double force) override;
384 
385  // Documentation inherited
386  double getForceUpperLimit(size_t index) const override;
387 
388  // Documentation inherited
389  void setForceUpperLimits(const Eigen::VectorXd& upperLimits) override;
390 
391  // Documentation inherited
392  Eigen::VectorXd getForceUpperLimits() const override;
393 
394  // Documentation inherited
395  void resetForces() override;
396 
398 
399  //----------------------------------------------------------------------------
401  //----------------------------------------------------------------------------
402 
403  // Documentation inherited
404  void setVelocityChange(std::size_t index, double velocityChange) override;
405 
406  // Documentation inherited
407  double getVelocityChange(std::size_t index) const override;
408 
409  // Documentation inherited
410  void resetVelocityChanges() override;
411 
413 
414  //----------------------------------------------------------------------------
416  //----------------------------------------------------------------------------
417 
418  // Documentation inherited
419  void setConstraintImpulse(std::size_t index, double impulse) override;
420 
421  // Documentation inherited
422  double getConstraintImpulse(std::size_t index) const override;
423 
424  // Documentation inherited
425  void resetConstraintImpulses() override;
426 
428 
429  //----------------------------------------------------------------------------
431  //----------------------------------------------------------------------------
432 
433  // Documentation inherited
434  void integratePositions(double dt) override;
435 
436  // Documentation inherited
437  void integrateVelocities(double dt) override;
438 
439  // Documentation inherited
440  Eigen::VectorXd getPositionDifferences(
441  const Eigen::VectorXd& q2, const Eigen::VectorXd& q1) const override;
442 
445  const Vector& q2, const Vector& q1) const;
446 
448 
449  //----------------------------------------------------------------------------
451  //----------------------------------------------------------------------------
452 
453  // Documentation inherited
454  void setSpringStiffness(std::size_t index, double k) override;
455 
456  // Documentation inherited
457  double getSpringStiffness(std::size_t index) const override;
458 
459  // Documentation inherited
460  void setRestPosition(std::size_t index, double q0) override;
461 
462  // Documentation inherited
463  double getRestPosition(std::size_t index) const override;
464 
465  // Documentation inherited
466  void setDampingCoefficient(std::size_t index, double coeff) override;
467 
468  // Documentation inherited
469  double getDampingCoefficient(std::size_t index) const override;
470 
471  // Documentation inherited
472  void setCoulombFriction(std::size_t index, double friction) override;
473 
474  // Documentation inherited
475  double getCoulombFriction(std::size_t index) const override;
476 
478 
479  //----------------------------------------------------------------------------
481  //----------------------------------------------------------------------------
482 
483  // Documentation inherited
484  double computePotentialEnergy() const override;
485 
487 
488  // Documentation inherited
489  Eigen::Vector6d getBodyConstraintWrench() const override;
490 
491  //----------------------------------------------------------------------------
493  //----------------------------------------------------------------------------
494 
495  // Documentation inherited
496  const math::Jacobian getRelativeJacobian() const override;
497 
499  const typename GenericJoint<ConfigSpaceT>::JacobianMatrix&
501 
502  // Documentation inherited
504  const Eigen::VectorXd& _positions) const override;
505 
508  const Vector& positions) const = 0;
509 
510  // Documentation inherited
511  const math::Jacobian getRelativeJacobianTimeDeriv() const override;
512 
515 
517 
518 protected:
519 
521 
522  // Documentation inherited
523  void registerDofs() override;
524 
525  //----------------------------------------------------------------------------
527  //----------------------------------------------------------------------------
528 
530  const Matrix& getInvProjArtInertia() const;
531 
534  const Matrix& getInvProjArtInertiaImplicit() const;
535 
536  // Documentation inherited
537  void updateRelativeSpatialVelocity() const override;
538 
539  // Documentation inherited
540  void updateRelativeSpatialAcceleration() const override;
541 
542  // Documentation inherited
543  void updateRelativePrimaryAcceleration() const override;
544 
545  // Documentation inherited
546  void addVelocityTo(Eigen::Vector6d& vel) override;
547 
548  // Documentation inherited
550  Eigen::Vector6d& partialAcceleration,
551  const Eigen::Vector6d& childVelocity) override;
552 
553  // Documentation inherited
554  void addAccelerationTo(Eigen::Vector6d& acc) override;
555 
556  // Documentation inherited
557  void addVelocityChangeTo(Eigen::Vector6d& velocityChange) override;
558 
559  // Documentation inherited
561  Eigen::Matrix6d& parentArtInertia,
562  const Eigen::Matrix6d& childArtInertia) override;
563 
564  // Documentation inherited
566  Eigen::Matrix6d& parentArtInertiaImplicit,
567  const Eigen::Matrix6d& childArtInertiaImplicit) override;
568 
569  // Documentation inherited
570  void updateInvProjArtInertia(const Eigen::Matrix6d& artInertia) override;
571 
572  // Documentation inherited
574  const Eigen::Matrix6d& artInertia, double timeStep) override;
575 
576  // Documentation inherited
577  void addChildBiasForceTo(
578  Eigen::Vector6d& parentBiasForce,
579  const Eigen::Matrix6d& childArtInertia,
580  const Eigen::Vector6d& childBiasForce,
581  const Eigen::Vector6d& childPartialAcc) override;
582 
583  // Documentation inherited
585  Eigen::Vector6d& parentBiasImpulse,
586  const Eigen::Matrix6d& childArtInertia,
587  const Eigen::Vector6d& childBiasImpulse) override;
588 
589  // Documentation inherited
590  void updateTotalForce(const Eigen::Vector6d& bodyForce,
591  double timeStep) override;
592 
593  // Documentation inherited
594  void updateTotalImpulse(const Eigen::Vector6d& bodyImpulse) override;
595 
596  // Documentation inherited
597  void resetTotalImpulses() override;
598 
599  // Documentation inherited
600  void updateAcceleration(const Eigen::Matrix6d& artInertia,
601  const Eigen::Vector6d& spatialAcc) override;
602 
603  // Documentation inherited
605  const Eigen::Matrix6d& artInertia,
606  const Eigen::Vector6d& velocityChange) override;
607 
608  // Documentation inherited
609  void updateForceID(const Eigen::Vector6d& bodyForce,
610  double timeStep,
611  bool withDampingForces,
612  bool withSpringForces) override;
613 
614  // Documentation inherited
615  void updateForceFD(const Eigen::Vector6d& bodyForce,
616  double timeStep,
617  bool withDampingForcese,
618  bool withSpringForces) override;
619 
620  // Documentation inherited
621  void updateImpulseID(const Eigen::Vector6d& bodyImpulse) override;
622 
623  // Documentation inherited
624  void updateImpulseFD(const Eigen::Vector6d& bodyImpulse) override;
625 
626  // Documentation inherited
627  void updateConstrainedTerms(double timeStep) override;
628 
630 
631  //----------------------------------------------------------------------------
633  //----------------------------------------------------------------------------
634 
635  // Documentation inherited
637  Eigen::Vector6d& parentBiasForce,
638  const Eigen::Matrix6d& childArtInertia,
639  const Eigen::Vector6d& childBiasForce) override;
640 
641  // Documentation inherited
643  Eigen::Vector6d& parentBiasForce,
644  const Eigen::Matrix6d& childArtInertia,
645  const Eigen::Vector6d& childBiasForce) override;
646 
647  // Documentation inherited
649  const Eigen::Vector6d& bodyForce) override;
650 
651  // Documentation inherited
652  void getInvMassMatrixSegment(Eigen::MatrixXd& invMassMat,
653  const size_t col,
654  const Eigen::Matrix6d& artInertia,
655  const Eigen::Vector6d& spatialAcc) override;
656 
657  // Documentation inherited
658  void getInvAugMassMatrixSegment(Eigen::MatrixXd& invMassMat,
659  const size_t col,
660  const Eigen::Matrix6d& artInertia,
661  const Eigen::Vector6d& spatialAcc) override;
662 
663  // Documentation inherited
664  void addInvMassMatrixSegmentTo(Eigen::Vector6d& acc) override;
665 
666  // Documentation inherited
667  Eigen::VectorXd getSpatialToGeneralized(
668  const Eigen::Vector6d& spatial) override;
669 
671 
672 protected:
673 
676 
677  //----------------------------------------------------------------------------
678  // Impulse
679  //----------------------------------------------------------------------------
680 
683 
686 
689 
690  //----------------------------------------------------------------------------
691  // For recursive dynamics algorithms
692  //----------------------------------------------------------------------------
693 
698 
704 
709 
716 
719 
722 
723  //----------------------------------------------------------------------------
724  // For equations of motion
725  //----------------------------------------------------------------------------
726 
729 
732 
733 private:
734  //----------------------------------------------------------------------------
736  //----------------------------------------------------------------------------
737 
739  Eigen::Matrix6d& parentArtInertia,
740  const Eigen::Matrix6d& childArtInertia);
741 
743  Eigen::Matrix6d& parentArtInertia,
744  const Eigen::Matrix6d& childArtInertia);
745 
747  Eigen::Matrix6d& parentArtInertia,
748  const Eigen::Matrix6d& childArtInertia);
749 
751  Eigen::Matrix6d& parentArtInertia,
752  const Eigen::Matrix6d& childArtInertia);
753 
755  const Eigen::Matrix6d& artInertia);
756 
758  const Eigen::Matrix6d& artInertia);
759 
761  const Eigen::Matrix6d& artInertia, double timeStep);
762 
764  const Eigen::Matrix6d& artInertia, double timeStep);
765 
767  Eigen::Vector6d& parentBiasForce,
768  const Eigen::Matrix6d& childArtInertia,
769  const Eigen::Vector6d& childBiasForce,
770  const Eigen::Vector6d& childPartialAcc);
771 
773  Eigen::Vector6d& parentBiasForce,
774  const Eigen::Matrix6d& childArtInertia,
775  const Eigen::Vector6d& childBiasForce,
776  const Eigen::Vector6d& childPartialAcc);
777 
779  Eigen::Vector6d& parentBiasImpulse,
780  const Eigen::Matrix6d& childArtInertia,
781  const Eigen::Vector6d& childBiasImpulse);
782 
784  Eigen::Vector6d& parentBiasImpulse,
785  const Eigen::Matrix6d& childArtInertia,
786  const Eigen::Vector6d& childBiasImpulse);
787 
788  void updateTotalForceDynamic(const Eigen::Vector6d& bodyForce,
789  double timeStep);
790 
791  void updateTotalForceKinematic(const Eigen::Vector6d& bodyForce,
792  double timeStep);
793 
795  const Eigen::Vector6d& bodyImpulse);
796 
798  const Eigen::Vector6d& bodyImpulse);
799 
801  const Eigen::Matrix6d& artInertia,
802  const Eigen::Vector6d& spatialAcc);
803 
805  const Eigen::Matrix6d& artInertia,
806  const Eigen::Vector6d& spatialAcc);
807 
809  const Eigen::Matrix6d& artInertia,
810  const Eigen::Vector6d& velocityChange);
811 
813  const Eigen::Matrix6d& artInertia,
814  const Eigen::Vector6d& velocityChange);
815 
816  void updateConstrainedTermsDynamic(double timeStep);
817 
818  void updateConstrainedTermsKinematic(double timeStep);
819 
821 };
822 
823 } // namespace dynamics
824 } // namespace dart
825 
826 #include "dart/dynamics/detail/GenericJoint.hpp"
827 
828 #endif // DART_DYNAMICS_GENERICJOINT_HPP_
BodyPropPtr properties
Definition: SdfParser.cpp:80
std::string * name
Definition: SkelParser.cpp:1642
bool * preserveName
Definition: SkelParser.cpp:1641
Eigen::VectorXd velocity
Definition: SkelParser.cpp:108
Eigen::VectorXd force
Definition: SkelParser.cpp:110
std::size_t index
Definition: SkelParser.cpp:1617
Eigen::VectorXd acceleration
Definition: SkelParser.cpp:109
double * friction
Definition: SkelParser.cpp:1639
Eigen::VectorXd position
Definition: SkelParser.cpp:107
Terminator for the variadic template.
Definition: CompositeJoiner.hpp:45
This is an alternative to EmbedStateAndProperties which allows your class to also inherit other Compo...
Definition: EmbeddedAspect.hpp:438
typename Impl::AspectState AspectState
Definition: EmbeddedAspect.hpp:444
typename Impl::Aspect Aspect
Definition: EmbeddedAspect.hpp:447
typename Impl::AspectProperties AspectProperties
Definition: EmbeddedAspect.hpp:446
DegreeOfFreedom class is a proxy class for accessing single degrees of freedom (aka generalized coord...
Definition: DegreeOfFreedom.hpp:53
Definition: GenericJoint.hpp:49
void setAspectState(const AspectState &state)
Set the AspectState of this GenericJoint.
Definition: GenericJoint.hpp:114
void setForceLowerLimits(const Eigen::VectorXd &lowerLimits) override
Definition: GenericJoint.hpp:1246
double getCommand(std::size_t index) const override
Definition: GenericJoint.hpp:391
typename ConfigSpaceT::Matrix Matrix
Definition: GenericJoint.hpp:61
void addChildArtInertiaImplicitToKinematic(Eigen::Matrix6d &parentArtInertia, const Eigen::Matrix6d &childArtInertia)
Definition: GenericJoint.hpp:1872
Eigen::VectorXd getCommands() const override
Definition: GenericJoint.hpp:473
void resetVelocities() override
Definition: GenericJoint.hpp:946
const Vector & getPositionsStatic() const
Fixed-size version of getPositions()
Definition: GenericJoint.hpp:732
void setAccelerationsStatic(const Vector &accels)
Fixed-size version of setAccelerations()
Definition: GenericJoint.hpp:759
void copy(const ThisClass &otherJoint)
Copy the Properties of another GenericJoint.
Definition: GenericJoint.hpp:159
void setForceUpperLimits(const Eigen::VectorXd &upperLimits) override
Definition: GenericJoint.hpp:1294
void updateTotalImpulseKinematic(const Eigen::Vector6d &bodyImpulse)
Definition: GenericJoint.hpp:2248
void setCoulombFriction(std::size_t index, double friction) override
Definition: GenericJoint.hpp:1536
const JacobianMatrix & getRelativeJacobianTimeDerivStatic() const
Fixed-size version of getRelativeJacobianTimeDeriv()
Definition: GenericJoint.hpp:1618
void setRestPosition(std::size_t index, double q0) override
Definition: GenericJoint.hpp:1469
Vector mTotalForce
Total force projected on joint space.
Definition: GenericJoint.hpp:718
void setAccelerations(const Eigen::VectorXd &accelerations) override
Definition: GenericJoint.hpp:1036
Vector mInvM_a
Definition: GenericJoint.hpp:728
void updateTotalImpulse(const Eigen::Vector6d &bodyImpulse) override
Definition: GenericJoint.hpp:2214
void updateForceFD(const Eigen::Vector6d &bodyForce, double timeStep, bool withDampingForcese, bool withSpringForces) override
Definition: GenericJoint.hpp:2393
const Matrix & getInvProjArtInertiaImplicit() const
Get the inverse of projected articulated inertia for implicit joint damping and spring forces.
Definition: GenericJoint.hpp:1761
Eigen::VectorXd getPositionUpperLimits() const override
Definition: GenericJoint.hpp:631
Vector mVelocityChanges
Change of generalized velocity.
Definition: GenericJoint.hpp:682
void setVelocityUpperLimits(const Eigen::VectorXd &upperLimits) override
Definition: GenericJoint.hpp:912
void updateForceID(const Eigen::Vector6d &bodyForce, double timeStep, bool withDampingForces, bool withSpringForces) override
Definition: GenericJoint.hpp:2362
Eigen::VectorXd getAccelerationUpperLimits() const override
Definition: GenericJoint.hpp:1153
void updateAccelerationKinematic(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc)
Definition: GenericJoint.hpp:2303
void updateVelocityChangeDynamic(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &velocityChange)
Definition: GenericJoint.hpp:2337
Eigen::VectorXd getVelocityLowerLimits() const override
Definition: GenericJoint.hpp:877
void updateInvProjArtInertiaImplicit(const Eigen::Matrix6d &artInertia, double timeStep) override
Definition: GenericJoint.hpp:1932
void setVelocityLowerLimits(const Eigen::VectorXd &lowerLimits) override
Definition: GenericJoint.hpp:863
void updateConstrainedTermsDynamic(double timeStep)
Definition: GenericJoint.hpp:2473
void setPositionLowerLimit(std::size_t index, double position) override
Definition: GenericJoint.hpp:542
const GenericJoint< ConfigSpaceT >::JacobianMatrix & getRelativeJacobianStatic() const
Fixed-size version of getRelativeJacobian()
Definition: GenericJoint.hpp:1588
void addChildArtInertiaToDynamic(Eigen::Matrix6d &parentArtInertia, const Eigen::Matrix6d &childArtInertia)
Definition: GenericJoint.hpp:1797
const math::Jacobian getRelativeJacobianTimeDeriv() const override
Definition: GenericJoint.hpp:1610
void setVelocities(const Eigen::VectorXd &velocities) override
Definition: GenericJoint.hpp:812
void addChildBiasImpulseToKinematic(Eigen::Vector6d &parentBiasImpulse, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasImpulse)
Definition: GenericJoint.hpp:2128
void addChildArtInertiaImplicitTo(Eigen::Matrix6d &parentArtInertiaImplicit, const Eigen::Matrix6d &childArtInertiaImplicit) override
Definition: GenericJoint.hpp:1827
void setPosition(std::size_t index, double position) override
Definition: GenericJoint.hpp:487
void updateVelocityChange(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &velocityChange) override
Definition: GenericJoint.hpp:2312
void setCommand(std::size_t index, double command) override
Definition: GenericJoint.hpp:326
std::array< DegreeOfFreedom *, NumDofs > mDofs
Array of DegreeOfFreedom objects.
Definition: GenericJoint.hpp:675
void setInitialPosition(std::size_t index, double initial) override
Definition: GenericJoint.hpp:672
Eigen::VectorXd getForces() const override
Definition: GenericJoint.hpp:1212
void resetCommands() override
Definition: GenericJoint.hpp:480
void setVelocityChange(std::size_t index, double velocityChange) override
Definition: GenericJoint.hpp:1325
void setAccelerationUpperLimits(const Eigen::VectorXd &upperLimits) override
Definition: GenericJoint.hpp:1139
void setPartialAccelerationTo(Eigen::Vector6d &partialAcceleration, const Eigen::Vector6d &childVelocity) override
Definition: GenericJoint.hpp:1712
double getVelocity(std::size_t index) const override
Definition: GenericJoint.hpp:799
Eigen::Vector6d getBodyConstraintWrench() const override
Definition: GenericJoint.hpp:1667
void addAccelerationTo(Eigen::Vector6d &acc) override
Definition: GenericJoint.hpp:1726
void updateTotalImpulseDynamic(const Eigen::Vector6d &bodyImpulse)
Definition: GenericJoint.hpp:2238
void setForce(std::size_t index, double force) override
Definition: GenericJoint.hpp:1167
Vector mConstraintImpulses
Generalized constraint impulse.
Definition: GenericJoint.hpp:688
void setInitialVelocity(std::size_t index, double initial) override
Definition: GenericJoint.hpp:953
double getForce(std::size_t index) const override
Definition: GenericJoint.hpp:1183
void setInitialVelocities(const Eigen::VectorXd &initial) override
Definition: GenericJoint.hpp:980
void updateInvProjArtInertiaDynamic(const Eigen::Matrix6d &artInertia)
Definition: GenericJoint.hpp:1908
DegreeOfFreedom * getDof(std::size_t index) override
void setPositionUpperLimit(std::size_t index, double position) override
Definition: GenericJoint.hpp:590
void updateInvProjArtInertiaImplicitKinematic(const Eigen::Matrix6d &artInertia, double timeStep)
Definition: GenericJoint.hpp:1980
const math::Jacobian getRelativeJacobian() const override
Definition: GenericJoint.hpp:1580
std::size_t getNumDofs() const override
Definition: GenericJoint.hpp:215
void resetForces() override
Definition: GenericJoint.hpp:1315
void addChildArtInertiaTo(Eigen::Matrix6d &parentArtInertia, const Eigen::Matrix6d &childArtInertia) override
Definition: GenericJoint.hpp:1770
Eigen::VectorXd getPositionLowerLimits() const override
Definition: GenericJoint.hpp:583
bool isDofNamePreserved(size_t index) const override
Definition: GenericJoint.hpp:269
void addVelocityTo(Eigen::Vector6d &vel) override
Definition: GenericJoint.hpp:1701
void preserveDofName(size_t index, bool preserve) override
Definition: GenericJoint.hpp:256
double getVelocityUpperLimit(std::size_t index) const override
Definition: GenericJoint.hpp:898
double getVelocityChange(std::size_t index) const override
Definition: GenericJoint.hpp:1339
Vector mTotalImpulse
Total impluse projected on joint space.
Definition: GenericJoint.hpp:721
void updateTotalForce(const Eigen::Vector6d &bodyForce, double timeStep) override
Definition: GenericJoint.hpp:2140
void resetConstraintImpulses() override
Definition: GenericJoint.hpp:1386
virtual Vector getPositionDifferencesStatic(const Vector &q2, const Vector &q1) const
Fixed-size version of getPositionDifferences()
Definition: GenericJoint.hpp:1432
void setVelocity(std::size_t index, double velocity) override
Definition: GenericJoint.hpp:778
double getRestPosition(std::size_t index) const override
Definition: GenericJoint.hpp:1494
void updateConstrainedTerms(double timeStep) override
Definition: GenericJoint.hpp:2450
void integrateVelocities(double dt) override
Definition: GenericJoint.hpp:1404
Eigen::VectorXd getPositions() const override
Definition: GenericJoint.hpp:535
void addChildBiasImpulseToDynamic(Eigen::Vector6d &parentBiasImpulse, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasImpulse)
Definition: GenericJoint.hpp:2107
double getAcceleration(std::size_t index) const override
Definition: GenericJoint.hpp:1023
Eigen::VectorXd getVelocities() const override
Definition: GenericJoint.hpp:829
Eigen::VectorXd getInitialPositions() const override
Definition: GenericJoint.hpp:713
Eigen::VectorXd getPositionDifferences(const Eigen::VectorXd &q2, const Eigen::VectorXd &q1) const override
Definition: GenericJoint.hpp:1413
void setPositionLowerLimits(const Eigen::VectorXd &lowerLimits) override
Definition: GenericJoint.hpp:569
double getAccelerationUpperLimit(std::size_t index) const override
Definition: GenericJoint.hpp:1125
Matrix mInvProjArtInertia
Inverse of projected articulated inertia.
Definition: GenericJoint.hpp:708
void setForceLowerLimit(size_t index, double force) override
Definition: GenericJoint.hpp:1219
void setSpringStiffness(std::size_t index, double k) override
Definition: GenericJoint.hpp:1441
void updateRelativePrimaryAcceleration() const override
Definition: GenericJoint.hpp:1693
void setProperties(const Properties &properties)
Set the Properties of this GenericJoint.
Definition: GenericJoint.hpp:98
double getForceLowerLimit(std::size_t index) const override
Definition: GenericJoint.hpp:1232
static constexpr std::size_t NumDofs
Definition: GenericJoint.hpp:52
const Vector & getVelocitiesStatic() const
Fixed-size version of getVelocities()
Definition: GenericJoint.hpp:752
double getPositionUpperLimit(std::size_t index) const override
Definition: GenericJoint.hpp:604
const std::string & getDofName(size_t index) const override
Definition: GenericJoint.hpp:282
double computePotentialEnergy() const override
Definition: GenericJoint.hpp:1565
double getSpringStiffness(std::size_t index) const override
Definition: GenericJoint.hpp:1456
void setDampingCoefficient(std::size_t index, double coeff) override
Definition: GenericJoint.hpp:1507
void updateInvProjArtInertia(const Eigen::Matrix6d &artInertia) override
Definition: GenericJoint.hpp:1884
void resetVelocity(std::size_t index) override
Definition: GenericJoint.hpp:933
JacobianMatrix mJacobianDeriv
Time derivative of spatial Jacobian expressed in the child body frame.
Definition: GenericJoint.hpp:703
void setPositionsStatic(const Vector &positions)
Fixed-size version of setPositions()
Definition: GenericJoint.hpp:720
void addChildBiasForceForInvAugMassMatrix(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce) override
Definition: GenericJoint.hpp:2515
void updateConstrainedTermsKinematic(double timeStep)
Definition: GenericJoint.hpp:2487
void setAccelerationLowerLimits(const Eigen::VectorXd &lowerLimits) override
Definition: GenericJoint.hpp:1089
void setAspectProperties(const AspectProperties &properties)
Set the AspectProperties of this GenericJoint.
Definition: GenericJoint.hpp:125
Eigen::VectorXd getForceUpperLimits() const override
Definition: GenericJoint.hpp:1308
void updateImpulseFD(const Eigen::Vector6d &bodyImpulse) override
Definition: GenericJoint.hpp:2427
void updateInvProjArtInertiaImplicitDynamic(const Eigen::Matrix6d &artInertia, double timeStep)
Definition: GenericJoint.hpp:1958
void updateTotalForceKinematic(const Eigen::Vector6d &bodyForce, double timeStep)
Definition: GenericJoint.hpp:2205
void resetTotalImpulses() override
Definition: GenericJoint.hpp:2256
void integratePositions(double dt) override
Definition: GenericJoint.hpp:1393
void setPositions(const Eigen::VectorXd &positions) override
Definition: GenericJoint.hpp:521
void addChildBiasForceToDynamic(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce, const Eigen::Vector6d &childPartialAcc)
Definition: GenericJoint.hpp:2021
Eigen::VectorXd getForceLowerLimits() const override
Definition: GenericJoint.hpp:1260
double getInitialVelocity(std::size_t index) const override
Definition: GenericJoint.hpp:967
Eigen::VectorXd getAccelerations() const override
Definition: GenericJoint.hpp:1053
typename ConfigSpaceT::Vector Vector
Definition: GenericJoint.hpp:59
void updateVelocityChangeKinematic(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &velocityChange)
Definition: GenericJoint.hpp:2353
const Vector & getAccelerationsStatic() const
Fixed-size version of getAccelerations()
Definition: GenericJoint.hpp:771
Eigen::VectorXd getInitialVelocities() const override
Definition: GenericJoint.hpp:994
void setCommands(const Eigen::VectorXd &commands) override
Definition: GenericJoint.hpp:404
Eigen::VectorXd getSpatialToGeneralized(const Eigen::Vector6d &spatial) override
Definition: GenericJoint.hpp:2602
typename ConfigSpaceT::JacobianMatrix JacobianMatrix
Definition: GenericJoint.hpp:60
void getInvAugMassMatrixSegment(Eigen::MatrixXd &invMassMat, const size_t col, const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc) override
Definition: GenericJoint.hpp:2569
double getVelocityLowerLimit(std::size_t index) const override
Definition: GenericJoint.hpp:850
void updateImpulseID(const Eigen::Vector6d &bodyImpulse) override
Definition: GenericJoint.hpp:2419
void setVelocityLowerLimit(std::size_t index, double velocity) override
Definition: GenericJoint.hpp:836
double getInitialPosition(std::size_t index) const override
Definition: GenericJoint.hpp:686
void updateAcceleration(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc) override
Definition: GenericJoint.hpp:2263
Vector mInvMassMatrixSegment
Definition: GenericJoint.hpp:731
void addChildBiasForceToKinematic(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce, const Eigen::Vector6d &childPartialAcc)
Definition: GenericJoint.hpp:2050
void setVelocityUpperLimit(std::size_t index, double velocity) override
Definition: GenericJoint.hpp:884
typename ConfigSpaceT::Point Point
Definition: GenericJoint.hpp:57
void updateInvProjArtInertiaKinematic(const Eigen::Matrix6d &artInertia)
Definition: GenericJoint.hpp:1924
bool hasPositionLimit(std::size_t index) const override
Definition: GenericJoint.hpp:638
JacobianMatrix mJacobian
Spatial Jacobian expressed in the child body frame.
Definition: GenericJoint.hpp:697
void setInitialPositions(const Eigen::VectorXd &initial) override
Definition: GenericJoint.hpp:699
void addChildBiasForceTo(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce, const Eigen::Vector6d &childPartialAcc) override
Definition: GenericJoint.hpp:1988
double getCoulombFriction(std::size_t index) const override
Definition: GenericJoint.hpp:1552
void addInvMassMatrixSegmentTo(Eigen::Vector6d &acc) override
Definition: GenericJoint.hpp:2593
Vector mImpulses
Generalized impulse.
Definition: GenericJoint.hpp:685
void setConstraintImpulse(std::size_t index, double impulse) override
Definition: GenericJoint.hpp:1359
void resetPosition(std::size_t index) override
Definition: GenericJoint.hpp:652
double getForceUpperLimit(size_t index) const override
Definition: GenericJoint.hpp:1280
void registerDofs() override
Definition: GenericJoint.hpp:1653
void addVelocityChangeTo(Eigen::Vector6d &velocityChange) override
Definition: GenericJoint.hpp:1738
const std::string & setDofName(std::size_t index, const std::string &name, bool preserveName=true) override
Definition: GenericJoint.hpp:222
void setAccelerationUpperLimit(std::size_t index, double acceleration) override
Definition: GenericJoint.hpp:1110
double getDampingCoefficient(std::size_t index) const override
Definition: GenericJoint.hpp:1523
Matrix mInvProjArtInertiaImplicit
Inverse of projected articulated inertia for implicit joint damping and spring forces.
Definition: GenericJoint.hpp:715
void setPositionUpperLimits(const Eigen::VectorXd &upperLimits) override
Definition: GenericJoint.hpp:617
Eigen::VectorXd getAccelerationLowerLimits() const override
Definition: GenericJoint.hpp:1103
void updateTotalForceDynamic(const Eigen::Vector6d &bodyForce, double timeStep)
Definition: GenericJoint.hpp:2180
void setVelocitiesStatic(const Vector &velocities)
Fixed-size version of setVelocities()
Definition: GenericJoint.hpp:739
void updateRelativeSpatialAcceleration() const override
Definition: GenericJoint.hpp:1684
void updateRelativeSpatialVelocity() const override
Definition: GenericJoint.hpp:1676
void setForceUpperLimit(size_t index, double force) override
Definition: GenericJoint.hpp:1267
size_t getIndexInTree(size_t index) const override
Definition: GenericJoint.hpp:313
Eigen::VectorXd getVelocityUpperLimits() const override
Definition: GenericJoint.hpp:926
void setAccelerationLowerLimit(size_t index, double acceleration) override
Definition: GenericJoint.hpp:1060
double getPosition(std::size_t index) const override
Definition: GenericJoint.hpp:508
void addChildArtInertiaImplicitToDynamic(Eigen::Matrix6d &parentArtInertia, const Eigen::Matrix6d &childArtInertia)
Definition: GenericJoint.hpp:1854
double getAccelerationLowerLimit(std::size_t index) const override
Definition: GenericJoint.hpp:1075
void addChildBiasImpulseTo(Eigen::Vector6d &parentBiasImpulse, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasImpulse) override
Definition: GenericJoint.hpp:2077
double getPositionLowerLimit(std::size_t index) const override
Definition: GenericJoint.hpp:556
void updateAccelerationDynamic(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc)
Definition: GenericJoint.hpp:2288
void resetPositions() override
Definition: GenericJoint.hpp:665
void addChildBiasForceForInvMassMatrix(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce) override
Definition: GenericJoint.hpp:2495
void getInvMassMatrixSegment(Eigen::MatrixXd &invMassMat, const size_t col, const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc) override
Definition: GenericJoint.hpp:2545
typename ConfigSpaceT::EuclideanPoint EuclideanPoint
Definition: GenericJoint.hpp:58
Properties getGenericJointProperties() const
Get the Properties of this GenericJoint.
Definition: GenericJoint.hpp:151
const Matrix & getInvProjArtInertia() const
Get the inverse of the projected articulated inertia.
Definition: GenericJoint.hpp:1751
void setForces(const Eigen::VectorXd &forces) override
Definition: GenericJoint.hpp:1196
void updateTotalForceForInvMassMatrix(const Eigen::Vector6d &bodyForce) override
Definition: GenericJoint.hpp:2535
void resetAccelerations() override
Definition: GenericJoint.hpp:1160
void setAcceleration(std::size_t index, double acceleration) override
Definition: GenericJoint.hpp:1001
void addChildArtInertiaToKinematic(Eigen::Matrix6d &parentArtInertia, const Eigen::Matrix6d &childArtInertia)
Definition: GenericJoint.hpp:1815
double getConstraintImpulse(std::size_t index) const override
Definition: GenericJoint.hpp:1373
size_t getIndexInSkeleton(size_t index) const override
Definition: GenericJoint.hpp:300
void resetVelocityChanges() override
Definition: GenericJoint.hpp:1352
#define DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(TypeName, AspectName)
Definition: Composite.hpp:164
Definition: Random-impl.hpp:92
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: SharedLibraryManager.hpp:43
Definition: GenericJointAspect.hpp:178
Definition: GenericJointAspect.hpp:87