33 #ifndef DART_DYNAMICS_GENERICJOINT_HPP_
34 #define DART_DYNAMICS_GENERICJOINT_HPP_
43 class DegreeOfFreedom;
45 template <
class ConfigSpaceT>
50 static constexpr std::size_t
NumDofs = ConfigSpaceT::NumDofs;
55 using Point =
typename ConfigSpaceT::Point;
57 using Vector =
typename ConfigSpaceT::Vector;
59 using Matrix =
typename ConfigSpaceT::Matrix;
441 const
Eigen::VectorXd& q2, const
Eigen::VectorXd& q1) const override;
504 const
Eigen::VectorXd& _positions) const override;
508 const
Vector& positions) const = 0;
573 const
Eigen::
Matrix6d& artInertia,
double timeStep) override;
612 bool withDampingForces,
613 bool withSpringForces) override;
619 bool withDampingForcese,
620 bool withSpringForces) override;
655 Eigen::MatrixXd& invMassMat,
662 Eigen::MatrixXd& invMassMat,
822 #include "dart/dynamics/detail/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