DART  6.10.1
GenericJoint.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, 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 <array>
37 #include <string>
39 
40 namespace dart {
41 namespace dynamics {
42 
43 class DegreeOfFreedom;
44 
45 template <class ConfigSpaceT>
47  : public detail::GenericJointBase<GenericJoint<ConfigSpaceT>, ConfigSpaceT>
48 {
49 public:
50  static constexpr std::size_t NumDofs = ConfigSpaceT::NumDofs;
51 
54 
55  using Point = typename ConfigSpaceT::Point;
56  using EuclideanPoint = typename ConfigSpaceT::EuclideanPoint;
57  using Vector = typename ConfigSpaceT::Vector;
58  using JacobianMatrix = typename ConfigSpaceT::JacobianMatrix;
59  using Matrix = typename ConfigSpaceT::Matrix;
60 
63  using AspectState = typename Base::AspectState;
65 
67  typename ThisClass::Aspect, GenericJointAspect)
68 
69  GenericJoint(const ThisClass&) = delete;
70 
72  virtual ~GenericJoint();
73 
75  void setProperties(const Properties& properties);
76 
79 
81  void setAspectState(const AspectState& state);
82 
85 
88 
90  void copy(const ThisClass& otherJoint);
91 
93  void copy(const ThisClass* otherJoint);
94 
96  ThisClass& operator=(const ThisClass& other);
97 
98  //----------------------------------------------------------------------------
100  //----------------------------------------------------------------------------
101 
102  // Documentation inherited
103  DegreeOfFreedom* getDof(std::size_t index) override;
104 
105  // Documentation inherited
106  const DegreeOfFreedom* getDof(std::size_t _index) const override;
107 
108  // Documentation inherited
109  std::size_t getNumDofs() const override;
110 
111  // Documentation inherited
112  const std::string& setDofName(
113  std::size_t index,
114  const std::string& name,
115  bool preserveName = true) override;
116 
117  // Documentation inherited
118  void preserveDofName(size_t index, bool preserve) override;
119 
120  // Documentation inherited
121  bool isDofNamePreserved(size_t index) const override;
122 
123  // Documentation inherited
124  const std::string& getDofName(size_t index) const override;
125 
126  // Documentation inherited
127  size_t getIndexInSkeleton(size_t index) const override;
128 
129  // Documentation inherited
130  size_t getIndexInTree(size_t index) const override;
131 
133 
134  //----------------------------------------------------------------------------
136  //----------------------------------------------------------------------------
137 
138  // Documentation inherited
139  void setCommand(std::size_t index, double command) override;
140 
141  // Documentation inherited
142  double getCommand(std::size_t index) const override;
143 
144  // Documentation inherited
145  void setCommands(const Eigen::VectorXd& commands) override;
146 
147  // Documentation inherited
148  Eigen::VectorXd getCommands() const override;
149 
150  // Documentation inherited
151  void resetCommands() override;
152 
154 
155  //----------------------------------------------------------------------------
157  //----------------------------------------------------------------------------
158 
159  // Documentation inherited
160  void setPosition(std::size_t index, double position) override;
161 
162  // Documentation inherited
163  double getPosition(std::size_t index) const override;
164 
165  // Documentation inherited
166  void setPositions(const Eigen::VectorXd& positions) override;
167 
168  // Documentation inherited
169  Eigen::VectorXd getPositions() const override;
170 
171  // Documentation inherited
172  void setPositionLowerLimit(std::size_t index, double position) override;
173 
174  // Documentation inherited
175  double getPositionLowerLimit(std::size_t index) const override;
176 
177  // Documentation inherited
178  void setPositionLowerLimits(const Eigen::VectorXd& lowerLimits) override;
179 
180  // Documentation inherited
181  Eigen::VectorXd getPositionLowerLimits() const override;
182 
183  // Documentation inherited
184  void setPositionUpperLimit(std::size_t index, double position) override;
185 
186  // Documentation inherited
187  double getPositionUpperLimit(std::size_t index) const override;
188 
189  // Documentation inherited
190  void setPositionUpperLimits(const Eigen::VectorXd& upperLimits) override;
191 
192  // Documentation inherited
193  Eigen::VectorXd getPositionUpperLimits() const override;
194 
195  // Documentation inherited
196  bool hasPositionLimit(std::size_t index) const override;
197 
198  // Documentation inherited
199  void resetPosition(std::size_t index) override;
200 
201  // Documentation inherited
202  void resetPositions() override;
203 
204  // Documentation inherited
205  void setInitialPosition(std::size_t index, double initial) override;
206 
207  // Documentation inherited
208  double getInitialPosition(std::size_t index) const override;
209 
210  // Documentation inherited
211  void setInitialPositions(const Eigen::VectorXd& initial) override;
212 
213  // Documentation inherited
214  Eigen::VectorXd getInitialPositions() const override;
215 
217 
218  //----------------------------------------------------------------------------
220  //----------------------------------------------------------------------------
221 
222  // Note: The fixed-size versions of these functions exist to make it easier
223  // to comply with the auto-updating design. Use these functions to avoid
224  // accessing mPosition directly, that way it is easier to ensure that the
225  // auto-updating design assumptions are being satisfied when reviewing the
226  // code.
227 
229  void setPositionsStatic(const Vector& positions);
230 
232  const Vector& getPositionsStatic() const;
233 
235  void setVelocitiesStatic(const Vector& velocities);
236 
238  const Vector& getVelocitiesStatic() const;
239 
241  void setAccelerationsStatic(const Vector& accels);
242 
244  const Vector& getAccelerationsStatic() const;
245 
247 
248  //----------------------------------------------------------------------------
250  //----------------------------------------------------------------------------
251 
252  // Documentation inherited
253  void setVelocity(std::size_t index, double velocity) override;
254 
255  // Documentation inherited
256  double getVelocity(std::size_t index) const override;
257 
258  // Documentation inherited
259  void setVelocities(const Eigen::VectorXd& velocities) override;
260 
261  // Documentation inherited
262  Eigen::VectorXd getVelocities() const override;
263 
264  // Documentation inherited
265  void setVelocityLowerLimit(std::size_t index, double velocity) override;
266 
267  // Documentation inherited
268  double getVelocityLowerLimit(std::size_t index) const override;
269 
270  // Documentation inherited
271  void setVelocityLowerLimits(const Eigen::VectorXd& lowerLimits) override;
272 
273  // Documentation inherited
274  Eigen::VectorXd getVelocityLowerLimits() const override;
275 
276  // Documentation inherited
277  void setVelocityUpperLimit(std::size_t index, double velocity) override;
278 
279  // Documentation inherited
280  double getVelocityUpperLimit(std::size_t index) const override;
281 
282  // Documentation inherited
283  void setVelocityUpperLimits(const Eigen::VectorXd& upperLimits) override;
284 
285  // Documentation inherited
286  Eigen::VectorXd getVelocityUpperLimits() const override;
287 
288  // Documentation inherited
289  void resetVelocity(std::size_t index) override;
290 
291  // Documentation inherited
292  void resetVelocities() override;
293 
294  // Documentation inherited
295  void setInitialVelocity(std::size_t index, double initial) override;
296 
297  // Documentation inherited
298  double getInitialVelocity(std::size_t index) const override;
299 
300  // Documentation inherited
301  void setInitialVelocities(const Eigen::VectorXd& initial) override;
302 
303  // Documentation inherited
304  Eigen::VectorXd getInitialVelocities() const override;
305 
307 
308  //----------------------------------------------------------------------------
310  //----------------------------------------------------------------------------
311 
312  // Documentation inherited
313  void setAcceleration(std::size_t index, double acceleration) override;
314 
315  // Documentation inherited
316  double getAcceleration(std::size_t index) const override;
317 
318  // Documentation inherited
319  void setAccelerations(const Eigen::VectorXd& accelerations) override;
320 
321  // Documentation inherited
322  Eigen::VectorXd getAccelerations() const override;
323 
324  // Documentation inherited
325  void setAccelerationLowerLimit(size_t index, double acceleration) override;
326 
327  // Documentation inherited
328  double getAccelerationLowerLimit(std::size_t index) const override;
329 
330  // Documentation inherited
331  void setAccelerationLowerLimits(const Eigen::VectorXd& lowerLimits) override;
332 
333  // Documentation inherited
334  Eigen::VectorXd getAccelerationLowerLimits() const override;
335 
336  // Documentation inherited
338  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:
520 
521  // Documentation inherited
522  void registerDofs() override;
523 
524  //----------------------------------------------------------------------------
526  //----------------------------------------------------------------------------
527 
529  const Matrix& getInvProjArtInertia() const;
530 
533  const Matrix& getInvProjArtInertiaImplicit() const;
534 
535  // Documentation inherited
536  void updateRelativeSpatialVelocity() const override;
537 
538  // Documentation inherited
539  void updateRelativeSpatialAcceleration() const override;
540 
541  // Documentation inherited
542  void updateRelativePrimaryAcceleration() const override;
543 
544  // Documentation inherited
545  void addVelocityTo(Eigen::Vector6d& vel) override;
546 
547  // Documentation inherited
549  Eigen::Vector6d& partialAcceleration,
550  const Eigen::Vector6d& childVelocity) override;
551 
552  // Documentation inherited
553  void addAccelerationTo(Eigen::Vector6d& acc) override;
554 
555  // Documentation inherited
556  void addVelocityChangeTo(Eigen::Vector6d& velocityChange) override;
557 
558  // Documentation inherited
560  Eigen::Matrix6d& parentArtInertia,
561  const Eigen::Matrix6d& childArtInertia) override;
562 
563  // Documentation inherited
565  Eigen::Matrix6d& parentArtInertiaImplicit,
566  const Eigen::Matrix6d& childArtInertiaImplicit) override;
567 
568  // Documentation inherited
569  void updateInvProjArtInertia(const Eigen::Matrix6d& artInertia) override;
570 
571  // Documentation inherited
573  const Eigen::Matrix6d& artInertia, double timeStep) override;
574 
575  // Documentation inherited
576  void addChildBiasForceTo(
577  Eigen::Vector6d& parentBiasForce,
578  const Eigen::Matrix6d& childArtInertia,
579  const Eigen::Vector6d& childBiasForce,
580  const Eigen::Vector6d& childPartialAcc) override;
581 
582  // Documentation inherited
584  Eigen::Vector6d& parentBiasImpulse,
585  const Eigen::Matrix6d& childArtInertia,
586  const Eigen::Vector6d& childBiasImpulse) override;
587 
588  // Documentation inherited
589  void updateTotalForce(
590  const Eigen::Vector6d& bodyForce, double timeStep) override;
591 
592  // Documentation inherited
593  void updateTotalImpulse(const Eigen::Vector6d& bodyImpulse) override;
594 
595  // Documentation inherited
596  void resetTotalImpulses() override;
597 
598  // Documentation inherited
599  void updateAcceleration(
600  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(
610  const Eigen::Vector6d& bodyForce,
611  double timeStep,
612  bool withDampingForces,
613  bool withSpringForces) override;
614 
615  // Documentation inherited
616  void updateForceFD(
617  const Eigen::Vector6d& bodyForce,
618  double timeStep,
619  bool withDampingForcese,
620  bool withSpringForces) override;
621 
622  // Documentation inherited
623  void updateImpulseID(const Eigen::Vector6d& bodyImpulse) override;
624 
625  // Documentation inherited
626  void updateImpulseFD(const Eigen::Vector6d& bodyImpulse) override;
627 
628  // Documentation inherited
629  void updateConstrainedTerms(double timeStep) override;
630 
632 
633  //----------------------------------------------------------------------------
635  //----------------------------------------------------------------------------
636 
637  // Documentation inherited
639  Eigen::Vector6d& parentBiasForce,
640  const Eigen::Matrix6d& childArtInertia,
641  const Eigen::Vector6d& childBiasForce) override;
642 
643  // Documentation inherited
645  Eigen::Vector6d& parentBiasForce,
646  const Eigen::Matrix6d& childArtInertia,
647  const Eigen::Vector6d& childBiasForce) override;
648 
649  // Documentation inherited
651  const Eigen::Vector6d& bodyForce) override;
652 
653  // Documentation inherited
655  Eigen::MatrixXd& invMassMat,
656  const size_t col,
657  const Eigen::Matrix6d& artInertia,
658  const Eigen::Vector6d& spatialAcc) override;
659 
660  // Documentation inherited
662  Eigen::MatrixXd& invMassMat,
663  const size_t col,
664  const Eigen::Matrix6d& artInertia,
665  const Eigen::Vector6d& spatialAcc) override;
666 
667  // Documentation inherited
668  void addInvMassMatrixSegmentTo(Eigen::Vector6d& acc) override;
669 
670  // Documentation inherited
671  Eigen::VectorXd getSpatialToGeneralized(
672  const Eigen::Vector6d& spatial) override;
673 
675 
676 protected:
679 
680  //----------------------------------------------------------------------------
681  // Impulse
682  //----------------------------------------------------------------------------
683 
686 
689 
692 
693  //----------------------------------------------------------------------------
694  // For recursive dynamics algorithms
695  //----------------------------------------------------------------------------
696 
702 
708 
713 
720 
723 
726 
727  //----------------------------------------------------------------------------
728  // For equations of motion
729  //----------------------------------------------------------------------------
730 
733 
736 
737 private:
738  //----------------------------------------------------------------------------
740  //----------------------------------------------------------------------------
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  Eigen::Matrix6d& parentArtInertia,
756  const Eigen::Matrix6d& childArtInertia);
757 
758  void updateInvProjArtInertiaDynamic(const Eigen::Matrix6d& artInertia);
759 
760  void updateInvProjArtInertiaKinematic(const Eigen::Matrix6d& artInertia);
761 
763  const Eigen::Matrix6d& artInertia, double timeStep);
764 
766  const Eigen::Matrix6d& artInertia, double timeStep);
767 
769  Eigen::Vector6d& parentBiasForce,
770  const Eigen::Matrix6d& childArtInertia,
771  const Eigen::Vector6d& childBiasForce,
772  const Eigen::Vector6d& childPartialAcc);
773 
775  Eigen::Vector6d& parentBiasForce,
776  const Eigen::Matrix6d& childArtInertia,
777  const Eigen::Vector6d& childBiasForce,
778  const Eigen::Vector6d& childPartialAcc);
779 
781  Eigen::Vector6d& parentBiasImpulse,
782  const Eigen::Matrix6d& childArtInertia,
783  const Eigen::Vector6d& childBiasImpulse);
784 
786  Eigen::Vector6d& parentBiasImpulse,
787  const Eigen::Matrix6d& childArtInertia,
788  const Eigen::Vector6d& childBiasImpulse);
789 
791  const Eigen::Vector6d& bodyForce, double timeStep);
792 
794  const Eigen::Vector6d& bodyForce, double timeStep);
795 
796  void updateTotalImpulseDynamic(const Eigen::Vector6d& bodyImpulse);
797 
798  void updateTotalImpulseKinematic(const Eigen::Vector6d& bodyImpulse);
799 
801  const Eigen::Matrix6d& artInertia, const Eigen::Vector6d& spatialAcc);
802 
804  const Eigen::Matrix6d& artInertia, const Eigen::Vector6d& spatialAcc);
805 
807  const Eigen::Matrix6d& artInertia, const Eigen::Vector6d& velocityChange);
808 
810  const Eigen::Matrix6d& artInertia, const Eigen::Vector6d& velocityChange);
811 
812  void updateConstrainedTermsDynamic(double timeStep);
813 
814  void updateConstrainedTermsKinematic(double timeStep);
815 
817 };
818 
819 } // namespace dynamics
820 } // namespace dart
821 
822 #include "dart/dynamics/detail/GenericJoint.hpp"
823 
824 #endif // DART_DYNAMICS_GENERICJOINT_HPP_
BodyPropPtr properties
Definition: SdfParser.cpp:80
std::string * name
Definition: SkelParser.cpp:1697
bool * preserveName
Definition: SkelParser.cpp:1696
Eigen::VectorXd velocity
Definition: SkelParser.cpp:109
Eigen::VectorXd force
Definition: SkelParser.cpp:111
std::size_t index
Definition: SkelParser.cpp:1672
Eigen::VectorXd acceleration
Definition: SkelParser.cpp:110
double * friction
Definition: SkelParser.cpp:1694
Eigen::VectorXd position
Definition: SkelParser.cpp:108
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:435
typename Impl::AspectState AspectState
Definition: EmbeddedAspect.hpp:440
typename Impl::Aspect Aspect
Definition: EmbeddedAspect.hpp:443
typename Impl::AspectProperties AspectProperties
Definition: EmbeddedAspect.hpp:442
DegreeOfFreedom class is a proxy class for accessing single degrees of freedom (aka generalized coord...
Definition: DegreeOfFreedom.hpp:55
Definition: GenericJoint.hpp:48
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:1240
double getCommand(std::size_t index) const override
Definition: GenericJoint.hpp:390
typename ConfigSpaceT::Matrix Matrix
Definition: GenericJoint.hpp:59
void addChildArtInertiaImplicitToKinematic(Eigen::Matrix6d &parentArtInertia, const Eigen::Matrix6d &childArtInertia)
Definition: GenericJoint.hpp:1842
Eigen::VectorXd getCommands() const override
Definition: GenericJoint.hpp:476
void resetVelocities() override
Definition: GenericJoint.hpp:946
const Vector & getPositionsStatic() const
Fixed-size version of getPositions()
Definition: GenericJoint.hpp:734
void setAccelerationsStatic(const Vector &accels)
Fixed-size version of setAccelerations()
Definition: GenericJoint.hpp:760
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:1287
void updateTotalImpulseKinematic(const Eigen::Vector6d &bodyImpulse)
Definition: GenericJoint.hpp:2205
void setCoulombFriction(std::size_t index, double friction) override
Definition: GenericJoint.hpp:1516
const JacobianMatrix & getRelativeJacobianTimeDerivStatic() const
Fixed-size version of getRelativeJacobianTimeDeriv()
Definition: GenericJoint.hpp:1600
void setRestPosition(std::size_t index, double q0) override
Definition: GenericJoint.hpp:1462
Vector mTotalForce
Total force projected on joint space.
Definition: GenericJoint.hpp:722
void setAccelerations(const Eigen::VectorXd &accelerations) override
Definition: GenericJoint.hpp:1036
Vector mInvM_a
Definition: GenericJoint.hpp:732
void updateTotalImpulse(const Eigen::Vector6d &bodyImpulse) override
Definition: GenericJoint.hpp:2171
void updateForceFD(const Eigen::Vector6d &bodyForce, double timeStep, bool withDampingForcese, bool withSpringForces) override
Definition: GenericJoint.hpp:2352
const Matrix & getInvProjArtInertiaImplicit() const
Get the inverse of projected articulated inertia for implicit joint damping and spring forces.
Definition: GenericJoint.hpp:1740
Eigen::VectorXd getPositionUpperLimits() const override
Definition: GenericJoint.hpp:633
Vector mVelocityChanges
Change of generalized velocity.
Definition: GenericJoint.hpp:685
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:2318
Eigen::VectorXd getAccelerationUpperLimits() const override
Definition: GenericJoint.hpp:1148
void updateAccelerationKinematic(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc)
Definition: GenericJoint.hpp:2260
void updateVelocityChangeDynamic(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &velocityChange)
Definition: GenericJoint.hpp:2293
Eigen::VectorXd getVelocityLowerLimits() const override
Definition: GenericJoint.hpp:878
void updateInvProjArtInertiaImplicit(const Eigen::Matrix6d &artInertia, double timeStep) override
Definition: GenericJoint.hpp:1901
void setVelocityLowerLimits(const Eigen::VectorXd &lowerLimits) override
Definition: GenericJoint.hpp:864
void updateConstrainedTermsDynamic(double timeStep)
Definition: GenericJoint.hpp:2432
void setPositionLowerLimit(std::size_t index, double position) override
Definition: GenericJoint.hpp:544
const GenericJoint< ConfigSpaceT >::JacobianMatrix & getRelativeJacobianStatic() const
Fixed-size version of getRelativeJacobian()
Definition: GenericJoint.hpp:1570
void addChildArtInertiaToDynamic(Eigen::Matrix6d &parentArtInertia, const Eigen::Matrix6d &childArtInertia)
Definition: GenericJoint.hpp:1773
const math::Jacobian getRelativeJacobianTimeDeriv() const override
Definition: GenericJoint.hpp:1591
void setVelocities(const Eigen::VectorXd &velocities) override
Definition: GenericJoint.hpp:813
void addChildBiasImpulseToKinematic(Eigen::Vector6d &parentBiasImpulse, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasImpulse)
Definition: GenericJoint.hpp:2090
void addChildArtInertiaImplicitTo(Eigen::Matrix6d &parentArtInertiaImplicit, const Eigen::Matrix6d &childArtInertiaImplicit) override
Definition: GenericJoint.hpp:1801
void setPosition(std::size_t index, double position) override
Definition: GenericJoint.hpp:490
void updateVelocityChange(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &velocityChange) override
Definition: GenericJoint.hpp:2269
void setCommand(std::size_t index, double command) override
Definition: GenericJoint.hpp:320
std::array< DegreeOfFreedom *, NumDofs > mDofs
Array of DegreeOfFreedom objects.
Definition: GenericJoint.hpp:678
void setInitialPosition(std::size_t index, double initial) override
Definition: GenericJoint.hpp:674
Eigen::VectorXd getForces() const override
Definition: GenericJoint.hpp:1207
void resetCommands() override
Definition: GenericJoint.hpp:483
void setVelocityChange(std::size_t index, double velocityChange) override
Definition: GenericJoint.hpp:1318
void setAccelerationUpperLimits(const Eigen::VectorXd &upperLimits) override
Definition: GenericJoint.hpp:1134
void setPartialAccelerationTo(Eigen::Vector6d &partialAcceleration, const Eigen::Vector6d &childVelocity) override
Definition: GenericJoint.hpp:1692
double getVelocity(std::size_t index) const override
Definition: GenericJoint.hpp:800
Eigen::Vector6d getBodyConstraintWrench() const override
Definition: GenericJoint.hpp:1647
void addAccelerationTo(Eigen::Vector6d &acc) override
Definition: GenericJoint.hpp:1706
void updateTotalImpulseDynamic(const Eigen::Vector6d &bodyImpulse)
Definition: GenericJoint.hpp:2195
void setForce(std::size_t index, double force) override
Definition: GenericJoint.hpp:1162
Vector mConstraintImpulses
Generalized constraint impulse.
Definition: GenericJoint.hpp:691
void setInitialVelocity(std::size_t index, double initial) override
Definition: GenericJoint.hpp:953
double getForce(std::size_t index) const override
Definition: GenericJoint.hpp:1178
void setInitialVelocities(const Eigen::VectorXd &initial) override
Definition: GenericJoint.hpp:980
void updateInvProjArtInertiaDynamic(const Eigen::Matrix6d &artInertia)
Definition: GenericJoint.hpp:1877
DegreeOfFreedom * getDof(std::size_t index) override
void setPositionUpperLimit(std::size_t index, double position) override
Definition: GenericJoint.hpp:592
void updateInvProjArtInertiaImplicitKinematic(const Eigen::Matrix6d &artInertia, double timeStep)
Definition: GenericJoint.hpp:1946
const math::Jacobian getRelativeJacobian() const override
Definition: GenericJoint.hpp:1562
std::size_t getNumDofs() const override
Definition: GenericJoint.hpp:212
void resetForces() override
Definition: GenericJoint.hpp:1308
void addChildArtInertiaTo(Eigen::Matrix6d &parentArtInertia, const Eigen::Matrix6d &childArtInertia) override
Definition: GenericJoint.hpp:1749
Eigen::VectorXd getPositionLowerLimits() const override
Definition: GenericJoint.hpp:585
bool isDofNamePreserved(size_t index) const override
Definition: GenericJoint.hpp:264
void addVelocityTo(Eigen::Vector6d &vel) override
Definition: GenericJoint.hpp:1681
void preserveDofName(size_t index, bool preserve) override
Definition: GenericJoint.hpp:251
double getVelocityUpperLimit(std::size_t index) const override
Definition: GenericJoint.hpp:899
double getVelocityChange(std::size_t index) const override
Definition: GenericJoint.hpp:1332
Vector mTotalImpulse
Total impluse projected on joint space.
Definition: GenericJoint.hpp:725
void updateTotalForce(const Eigen::Vector6d &bodyForce, double timeStep) override
Definition: GenericJoint.hpp:2103
void resetConstraintImpulses() override
Definition: GenericJoint.hpp:1379
virtual Vector getPositionDifferencesStatic(const Vector &q2, const Vector &q1) const
Fixed-size version of getPositionDifferences()
Definition: GenericJoint.hpp:1425
void setVelocity(std::size_t index, double velocity) override
Definition: GenericJoint.hpp:779
double getRestPosition(std::size_t index) const override
Definition: GenericJoint.hpp:1475
void updateConstrainedTerms(double timeStep) override
Definition: GenericJoint.hpp:2409
void integrateVelocities(double dt) override
Definition: GenericJoint.hpp:1398
Eigen::VectorXd getPositions() const override
Definition: GenericJoint.hpp:537
void addChildBiasImpulseToDynamic(Eigen::Vector6d &parentBiasImpulse, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasImpulse)
Definition: GenericJoint.hpp:2070
double getAcceleration(std::size_t index) const override
Definition: GenericJoint.hpp:1023
Eigen::VectorXd getVelocities() const override
Definition: GenericJoint.hpp:830
Eigen::VectorXd getInitialPositions() const override
Definition: GenericJoint.hpp:715
Eigen::VectorXd getPositionDifferences(const Eigen::VectorXd &q2, const Eigen::VectorXd &q1) const override
Definition: GenericJoint.hpp:1406
void setPositionLowerLimits(const Eigen::VectorXd &lowerLimits) override
Definition: GenericJoint.hpp:571
double getAccelerationUpperLimit(std::size_t index) const override
Definition: GenericJoint.hpp:1121
Matrix mInvProjArtInertia
Inverse of projected articulated inertia.
Definition: GenericJoint.hpp:712
void setForceLowerLimit(size_t index, double force) override
Definition: GenericJoint.hpp:1214
void setSpringStiffness(std::size_t index, double k) override
Definition: GenericJoint.hpp:1434
void updateRelativePrimaryAcceleration() const override
Definition: GenericJoint.hpp:1673
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:1227
static constexpr std::size_t NumDofs
Definition: GenericJoint.hpp:50
const Vector & getVelocitiesStatic() const
Fixed-size version of getVelocities()
Definition: GenericJoint.hpp:753
double getPositionUpperLimit(std::size_t index) const override
Definition: GenericJoint.hpp:606
const std::string & getDofName(size_t index) const override
Definition: GenericJoint.hpp:277
double computePotentialEnergy() const override
Definition: GenericJoint.hpp:1545
double getSpringStiffness(std::size_t index) const override
Definition: GenericJoint.hpp:1449
void setDampingCoefficient(std::size_t index, double coeff) override
Definition: GenericJoint.hpp:1488
void updateInvProjArtInertia(const Eigen::Matrix6d &artInertia) override
Definition: GenericJoint.hpp:1853
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:707
void setPositionsStatic(const Vector &positions)
Fixed-size version of setPositions()
Definition: GenericJoint.hpp:722
void addChildBiasForceForInvAugMassMatrix(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce) override
Definition: GenericJoint.hpp:2473
void updateConstrainedTermsKinematic(double timeStep)
Definition: GenericJoint.hpp:2445
void setAccelerationLowerLimits(const Eigen::VectorXd &lowerLimits) override
Definition: GenericJoint.hpp:1087
void setAspectProperties(const AspectProperties &properties)
Set the AspectProperties of this GenericJoint.
Definition: GenericJoint.hpp:125
Eigen::VectorXd getForceUpperLimits() const override
Definition: GenericJoint.hpp:1301
void updateImpulseFD(const Eigen::Vector6d &bodyImpulse) override
Definition: GenericJoint.hpp:2386
void updateInvProjArtInertiaImplicitDynamic(const Eigen::Matrix6d &artInertia, double timeStep)
Definition: GenericJoint.hpp:1925
void updateTotalForceKinematic(const Eigen::Vector6d &bodyForce, double timeStep)
Definition: GenericJoint.hpp:2163
void resetTotalImpulses() override
Definition: GenericJoint.hpp:2213
void integratePositions(double dt) override
Definition: GenericJoint.hpp:1386
void setPositions(const Eigen::VectorXd &positions) override
Definition: GenericJoint.hpp:524
void addChildBiasForceToDynamic(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce, const Eigen::Vector6d &childPartialAcc)
Definition: GenericJoint.hpp:1983
Eigen::VectorXd getForceLowerLimits() const override
Definition: GenericJoint.hpp:1254
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:57
void updateVelocityChangeKinematic(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &velocityChange)
Definition: GenericJoint.hpp:2309
const Vector & getAccelerationsStatic() const
Fixed-size version of getAccelerations()
Definition: GenericJoint.hpp:772
Eigen::VectorXd getInitialVelocities() const override
Definition: GenericJoint.hpp:994
void setCommands(const Eigen::VectorXd &commands) override
Definition: GenericJoint.hpp:403
Eigen::VectorXd getSpatialToGeneralized(const Eigen::Vector6d &spatial) override
Definition: GenericJoint.hpp:2561
typename ConfigSpaceT::JacobianMatrix JacobianMatrix
Definition: GenericJoint.hpp:58
void getInvAugMassMatrixSegment(Eigen::MatrixXd &invMassMat, const size_t col, const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc) override
Definition: GenericJoint.hpp:2528
double getVelocityLowerLimit(std::size_t index) const override
Definition: GenericJoint.hpp:851
void updateImpulseID(const Eigen::Vector6d &bodyImpulse) override
Definition: GenericJoint.hpp:2378
void setVelocityLowerLimit(std::size_t index, double velocity) override
Definition: GenericJoint.hpp:837
double getInitialPosition(std::size_t index) const override
Definition: GenericJoint.hpp:688
void updateAcceleration(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc) override
Definition: GenericJoint.hpp:2220
Vector mInvMassMatrixSegment
Definition: GenericJoint.hpp:735
void addChildBiasForceToKinematic(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce, const Eigen::Vector6d &childPartialAcc)
Definition: GenericJoint.hpp:2013
void setVelocityUpperLimit(std::size_t index, double velocity) override
Definition: GenericJoint.hpp:885
typename ConfigSpaceT::Point Point
Definition: GenericJoint.hpp:55
void updateInvProjArtInertiaKinematic(const Eigen::Matrix6d &artInertia)
Definition: GenericJoint.hpp:1893
bool hasPositionLimit(std::size_t index) const override
Definition: GenericJoint.hpp:640
JacobianMatrix mJacobian
Spatial Jacobian expressed in the child body frame.
Definition: GenericJoint.hpp:701
void setInitialPositions(const Eigen::VectorXd &initial) override
Definition: GenericJoint.hpp:701
void addChildBiasForceTo(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce, const Eigen::Vector6d &childPartialAcc) override
Definition: GenericJoint.hpp:1954
double getCoulombFriction(std::size_t index) const override
Definition: GenericJoint.hpp:1532
void addInvMassMatrixSegmentTo(Eigen::Vector6d &acc) override
Definition: GenericJoint.hpp:2553
Vector mImpulses
Generalized impulse.
Definition: GenericJoint.hpp:688
void setConstraintImpulse(std::size_t index, double impulse) override
Definition: GenericJoint.hpp:1352
void resetPosition(std::size_t index) override
Definition: GenericJoint.hpp:654
double getForceUpperLimit(size_t index) const override
Definition: GenericJoint.hpp:1274
void registerDofs() override
Definition: GenericJoint.hpp:1634
void addVelocityChangeTo(Eigen::Vector6d &velocityChange) override
Definition: GenericJoint.hpp:1717
const std::string & setDofName(std::size_t index, const std::string &name, bool preserveName=true) override
Definition: GenericJoint.hpp:219
void setAccelerationUpperLimit(std::size_t index, double acceleration) override
Definition: GenericJoint.hpp:1108
double getDampingCoefficient(std::size_t index) const override
Definition: GenericJoint.hpp:1503
Matrix mInvProjArtInertiaImplicit
Inverse of projected articulated inertia for implicit joint damping and spring forces.
Definition: GenericJoint.hpp:719
void setPositionUpperLimits(const Eigen::VectorXd &upperLimits) override
Definition: GenericJoint.hpp:619
Eigen::VectorXd getAccelerationLowerLimits() const override
Definition: GenericJoint.hpp:1101
void updateTotalForceDynamic(const Eigen::Vector6d &bodyForce, double timeStep)
Definition: GenericJoint.hpp:2142
void setVelocitiesStatic(const Vector &velocities)
Fixed-size version of setVelocities()
Definition: GenericJoint.hpp:741
void updateRelativeSpatialAcceleration() const override
Definition: GenericJoint.hpp:1664
void updateRelativeSpatialVelocity() const override
Definition: GenericJoint.hpp:1656
void setForceUpperLimit(size_t index, double force) override
Definition: GenericJoint.hpp:1261
size_t getIndexInTree(size_t index) const override
Definition: GenericJoint.hpp:307
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:511
void addChildArtInertiaImplicitToDynamic(Eigen::Matrix6d &parentArtInertia, const Eigen::Matrix6d &childArtInertia)
Definition: GenericJoint.hpp:1825
double getAccelerationLowerLimit(std::size_t index) const override
Definition: GenericJoint.hpp:1074
void addChildBiasImpulseTo(Eigen::Vector6d &parentBiasImpulse, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasImpulse) override
Definition: GenericJoint.hpp:2042
double getPositionLowerLimit(std::size_t index) const override
Definition: GenericJoint.hpp:558
void updateAccelerationDynamic(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc)
Definition: GenericJoint.hpp:2244
void resetPositions() override
Definition: GenericJoint.hpp:667
void addChildBiasForceForInvMassMatrix(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce) override
Definition: GenericJoint.hpp:2453
void getInvMassMatrixSegment(Eigen::MatrixXd &invMassMat, const size_t col, const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc) override
Definition: GenericJoint.hpp:2503
typename ConfigSpaceT::EuclideanPoint EuclideanPoint
Definition: GenericJoint.hpp:56
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:1730
void setForces(const Eigen::VectorXd &forces) override
Definition: GenericJoint.hpp:1191
void updateTotalForceForInvMassMatrix(const Eigen::Vector6d &bodyForce) override
Definition: GenericJoint.hpp:2493
void resetAccelerations() override
Definition: GenericJoint.hpp:1155
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:1790
double getConstraintImpulse(std::size_t index) const override
Definition: GenericJoint.hpp:1366
size_t getIndexInSkeleton(size_t index) const override
Definition: GenericJoint.hpp:294
void resetVelocityChanges() override
Definition: GenericJoint.hpp:1345
#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:114
Definition: BulletCollisionDetector.cpp:65
Definition: SharedLibraryManager.hpp:46
Definition: GenericJointAspect.hpp:191
Definition: GenericJointAspect.hpp:88