33 #ifndef DART_DYNAMICS_ZERODOFJOINT_HPP_
34 #define DART_DYNAMICS_ZERODOFJOINT_HPP_
75 const std::string&
setDofName(std::size_t,
const std::string&,
bool)
override;
83 const std::string&
getDofName(std::size_t)
const override;
99 void setCommand(std::size_t _index,
double _command)
override;
102 double getCommand(std::size_t _index)
const override;
105 void setCommands(
const Eigen::VectorXd& _commands)
override;
121 double getPosition(std::size_t _index)
const override;
124 void setPositions(
const Eigen::VectorXd& _positions)
override;
179 void setVelocity(std::size_t _index,
double _velocity)
override;
182 double getVelocity(std::size_t _index)
const override;
185 void setVelocities(
const Eigen::VectorXd& _velocities)
override;
237 void setAcceleration(std::size_t _index,
double _acceleration)
override;
253 std::size_t _index,
double _acceleration)
override;
266 std::size_t _index,
double _acceleration)
override;
282 void setForce(std::size_t _index,
double _force)
override;
285 double getForce(std::size_t _index)
const override;
288 void setForces(
const Eigen::VectorXd& _forces)
override;
291 Eigen::VectorXd
getForces()
const override;
358 const Eigen::VectorXd& _q2,
const Eigen::VectorXd& _q1)
const override;
417 const Eigen::VectorXd& _positions)
const override;
490 bool _withDampingForces,
491 bool _withSpringForces)
override;
497 bool _withDampingForces,
498 bool _withSpringForces)
override;
533 Eigen::MatrixXd& _invMassMat,
534 const std::size_t _col,
540 Eigen::MatrixXd& _invMassMat,
541 const std::size_t _col,
std::size_t index
Definition: SkelParser.cpp:1672
Eigen::VectorXd position
Definition: SkelParser.cpp:108
DegreeOfFreedom class is a proxy class for accessing single degrees of freedom (aka generalized coord...
Definition: DegreeOfFreedom.hpp:55
class Joint
Definition: Joint.hpp:60
class ZeroDofJoint
Definition: ZeroDofJoint.hpp:48
const std::string & setDofName(std::size_t, const std::string &, bool) override
Alternative to DegreeOfFreedom::setName()
Definition: ZeroDofJoint.cpp:81
double getPositionLowerLimit(std::size_t _index) const override
Get lower limit for position.
Definition: ZeroDofJoint.cpp:198
void addVelocityChangeTo(Eigen::Vector6d &_velocityChange) override
Add joint velocity change to _velocityChange.
Definition: ZeroDofJoint.cpp:722
void addChildBiasForceForInvAugMassMatrix(Eigen::Vector6d &_parentBiasForce, const Eigen::Matrix6d &_childArtInertia, const Eigen::Vector6d &_childBiasForce) override
Add child's bias force to parent's one.
Definition: ZeroDofJoint.cpp:884
Eigen::VectorXd getVelocities() const override
Get the velocities of all generalized coordinates in this Joint.
Definition: ZeroDofJoint.cpp:304
const std::string & getDofName(std::size_t) const override
Alternative to DegreeOfFreedom::getName()
Definition: ZeroDofJoint.cpp:100
void setForceLowerLimits(const Eigen::VectorXd &lowerLimits) override
Set the force upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:524
void updateImpulseFD(const Eigen::Vector6d &_bodyImpulse) override
Update joint impulses for forward dynamics.
Definition: ZeroDofJoint.cpp:863
std::size_t getIndexInTree(std::size_t _index) const override
Get a unique index in the kinematic tree of a generalized coordinate in this Joint.
Definition: ZeroDofJoint.cpp:122
void setAccelerationUpperLimit(std::size_t _index, double _acceleration) override
Set upper limit for acceleration.
Definition: ZeroDofJoint.cpp:456
bool isDofNamePreserved(std::size_t) const override
Alternative to DegreeOfFreedom::isNamePreserved()
Definition: ZeroDofJoint.cpp:94
double getConstraintImpulse(std::size_t _index) const override
Get a single constraint impulse.
Definition: ZeroDofJoint.cpp:586
void setCommands(const Eigen::VectorXd &_commands) override
Set all commands for this Joint.
Definition: ZeroDofJoint.cpp:147
void setCoulombFriction(std::size_t _index, double _friction) override
Set joint Coulomb friction froce.
Definition: ZeroDofJoint.cpp:653
void addInvMassMatrixSegmentTo(Eigen::Vector6d &_acc) override
Definition: ZeroDofJoint.cpp:920
void addChildBiasForceForInvMassMatrix(Eigen::Vector6d &_parentBiasForce, const Eigen::Matrix6d &_childArtInertia, const Eigen::Vector6d &_childBiasForce) override
Add child's bias force to parent's one.
Definition: ZeroDofJoint.cpp:875
Eigen::VectorXd getPositionLowerLimits() const override
Get the position lower limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:211
double getCommand(std::size_t _index) const override
Get a single command.
Definition: ZeroDofJoint.cpp:138
double getSpringStiffness(std::size_t _index) const override
Get stiffness of joint spring force.
Definition: ZeroDofJoint.cpp:623
void setVelocityChange(std::size_t _index, double _velocityChange) override
Set a single velocity change.
Definition: ZeroDofJoint.cpp:560
double getVelocity(std::size_t _index) const override
Get the velocity of a single generalized coordinate.
Definition: ZeroDofJoint.cpp:292
void setPartialAccelerationTo(Eigen::Vector6d &_partialAcceleration, const Eigen::Vector6d &_childVelocity) override
Set joint partial acceleration to _partialAcceleration.
Definition: ZeroDofJoint.cpp:728
void setConstraintImpulse(std::size_t _index, double _impulse) override
Set a single constraint impulse.
Definition: ZeroDofJoint.cpp:579
Eigen::VectorXd getPositions() const override
Get the positions of all generalized coordinates in this Joint.
Definition: ZeroDofJoint.cpp:185
const math::Jacobian getRelativeJacobian() const override
Get spatial Jacobian of the child BodyNode relative to the parent BodyNode expressed in the child Bod...
Definition: ZeroDofJoint.cpp:697
void addChildArtInertiaTo(Eigen::Matrix6d &_parentArtInertia, const Eigen::Matrix6d &_childArtInertia) override
Add child's articulated inertia to parent's one.
Definition: ZeroDofJoint.cpp:742
void setInitialPositions(const Eigen::VectorXd &_initial) override
Change the positions that resetPositions() will give to this Joint's coordinates.
Definition: ZeroDofJoint.cpp:274
void preserveDofName(std::size_t, bool) override
Alternative to DegreeOfFreedom::preserveName()
Definition: ZeroDofJoint.cpp:88
void addChildBiasImpulseTo(Eigen::Vector6d &_parentBiasImpulse, const Eigen::Matrix6d &_childArtInertia, const Eigen::Vector6d &_childBiasImpulse) override
Add child's bias impulse to parent's one.
Definition: ZeroDofJoint.cpp:790
double getVelocityLowerLimit(std::size_t _index) const override
Get lower limit for velocity.
Definition: ZeroDofJoint.cpp:317
double computePotentialEnergy() const override
Compute and return the potential energy.
Definition: ZeroDofJoint.cpp:666
void setPositionUpperLimits(const Eigen::VectorXd &upperLimits) override
Set the position upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:230
void addChildBiasForceTo(Eigen::Vector6d &_parentBiasForce, const Eigen::Matrix6d &_childArtInertia, const Eigen::Vector6d &_childBiasForce, const Eigen::Vector6d &_childPartialAcc) override
Add child's bias force to parent's one.
Definition: ZeroDofJoint.cpp:776
void setForceUpperLimits(const Eigen::VectorXd &upperLimits) override
Set the force upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:548
double getRestPosition(std::size_t _index) const override
Get rest position of spring force.
Definition: ZeroDofJoint.cpp:635
double getForceLowerLimit(std::size_t _index) const override
Get lower limit for force.
Definition: ZeroDofJoint.cpp:518
void integrateVelocities(double _dt) override
Integrate velocities using Euler method.
Definition: ZeroDofJoint.cpp:604
Eigen::VectorXd getForceUpperLimits() const override
Get the force upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:554
Eigen::VectorXd getAccelerationLowerLimits() const override
Get the acceleration upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:450
void setVelocityUpperLimit(std::size_t _index, double _velocity) override
Set upper limit for velocity.
Definition: ZeroDofJoint.cpp:336
void updateForceID(const Eigen::Vector6d &_bodyForce, double _timeStep, bool _withDampingForces, bool _withSpringForces) override
Update joint force for inverse dynamics.
Definition: ZeroDofJoint.cpp:837
void updateImpulseID(const Eigen::Vector6d &_bodyImpulse) override
Update joint impulses for inverse dynamics.
Definition: ZeroDofJoint.cpp:857
void addChildArtInertiaImplicitTo(Eigen::Matrix6d &_parentArtInertia, const Eigen::Matrix6d &_childArtInertia) override
Add child's articulated inertia to parent's one. Forward dynamics routine.
Definition: ZeroDofJoint.cpp:752
Properties getZeroDofJointProperties() const
Get the Properties of this ZeroDofJoint.
Definition: ZeroDofJoint.cpp:57
ZeroDofJoint()
Constructor called by inheriting classes.
Definition: ZeroDofJoint.cpp:672
void setAccelerationUpperLimits(const Eigen::VectorXd &upperLimits) override
Set the acceleration upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:469
void addAccelerationTo(Eigen::Vector6d &_acc) override
Add joint acceleration to _acc.
Definition: ZeroDofJoint.cpp:736
void resetCommands() override
Set all the commands for this Joint to zero.
Definition: ZeroDofJoint.cpp:159
Eigen::VectorXd getForceLowerLimits() const override
Get the force upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:530
std::size_t getNumDofs() const override
Get number of generalized coordinates.
Definition: ZeroDofJoint.cpp:106
Eigen::Vector6d getBodyConstraintWrench() const override
Get constraint wrench expressed in body node frame.
Definition: ZeroDofJoint.cpp:690
const std::string emptyString
Used by getDofName()
Definition: ZeroDofJoint.hpp:556
void setInitialVelocities(const Eigen::VectorXd &_initial) override
Change the velocities that resetVelocities() will give to this Joint's coordinates.
Definition: ZeroDofJoint.cpp:387
void resetConstraintImpulses() override
Set zero all the constraint impulses.
Definition: ZeroDofJoint.cpp:592
void getInvAugMassMatrixSegment(Eigen::MatrixXd &_invMassMat, const std::size_t _col, const Eigen::Matrix6d &_artInertia, const Eigen::Vector6d &_spatialAcc) override
Definition: ZeroDofJoint.cpp:910
void setForce(std::size_t _index, double _force) override
Set the force of a single generalized coordinate.
Definition: ZeroDofJoint.cpp:482
double getAccelerationLowerLimit(std::size_t _index) const override
Get lower limit for acceleration.
Definition: ZeroDofJoint.cpp:437
void setPosition(std::size_t, double) override
Set the position of a single generalized coordinate.
Definition: ZeroDofJoint.cpp:165
double getForce(std::size_t _index) const override
Get the force of a single generalized coordinate.
Definition: ZeroDofJoint.cpp:488
void setVelocityLowerLimit(std::size_t _index, double _velocity) override
Set lower limit for velocity.
Definition: ZeroDofJoint.cpp:310
void resetVelocityChanges() override
Set zero all the velocity change.
Definition: ZeroDofJoint.cpp:573
void registerDofs() override
Called by the Skeleton class.
Definition: ZeroDofJoint.cpp:678
Eigen::VectorXd getSpatialToGeneralized(const Eigen::Vector6d &_spatial) override
Definition: ZeroDofJoint.cpp:926
double getVelocityChange(std::size_t _index) const override
Get a single velocity change.
Definition: ZeroDofJoint.cpp:567
void setSpringStiffness(std::size_t _index, double _k) override
Set stiffness of joint spring force.
Definition: ZeroDofJoint.cpp:617
void resetPosition(std::size_t _index) override
Set the position of this generalized coordinate to its initial position.
Definition: ZeroDofJoint.cpp:249
void setCommand(std::size_t _index, double _command) override
Set a single command.
Definition: ZeroDofJoint.cpp:132
Eigen::VectorXd getAccelerations() const override
Get the accelerations of all generalized coordinates in this Joint.
Definition: ZeroDofJoint.cpp:418
void addVelocityTo(Eigen::Vector6d &_vel) override
Add joint velocity to _vel.
Definition: ZeroDofJoint.cpp:716
double getInitialVelocity(std::size_t _index) const override
Get the velocity that resetVelocity() will give to this coordinate.
Definition: ZeroDofJoint.cpp:381
void setRestPosition(std::size_t _index, double _q0) override
Set rest position of spring force.
Definition: ZeroDofJoint.cpp:629
Eigen::VectorXd getCommands() const override
Get all commands for this Joint.
Definition: ZeroDofJoint.cpp:153
void setAcceleration(std::size_t _index, double _acceleration) override
Set the acceleration of a single generalized coordinate.
Definition: ZeroDofJoint.cpp:399
std::size_t getIndexInSkeleton(std::size_t _index) const override
Get a unique index in skeleton of a generalized coordinate in this Joint.
Definition: ZeroDofJoint.cpp:112
void resetForces() override
Set the forces of all generalized coordinates in this Joint to zero.
Definition: ZeroDofJoint.cpp:506
void setInitialVelocity(std::size_t _index, double _initial) override
Change the velocity that resetVelocity() will give to this coordinate.
Definition: ZeroDofJoint.cpp:374
void updateVelocityChange(const Eigen::Matrix6d &_artInertia, const Eigen::Vector6d &_velocityChange) override
Update joint velocity change.
Definition: ZeroDofJoint.cpp:829
void setPositionLowerLimits(const Eigen::VectorXd &lowerLimits) override
Set the position lower limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:204
void updateForceFD(const Eigen::Vector6d &_bodyForce, double _timeStep, bool _withDampingForces, bool _withSpringForces) override
Update joint force for forward dynamics.
Definition: ZeroDofJoint.cpp:847
void resetVelocities() override
Set the velocities of all generalized coordinates in this Joint to their initial velocities.
Definition: ZeroDofJoint.cpp:368
void updateInvProjArtInertiaImplicit(const Eigen::Matrix6d &_artInertia, double _timeStep) override
Forward dynamics routine.
Definition: ZeroDofJoint.cpp:769
void resetPositions() override
Set the positions of all generalized coordinates in this Joint to their initial positions.
Definition: ZeroDofJoint.cpp:255
void updateTotalImpulse(const Eigen::Vector6d &_bodyImpulse) override
Update joint total impulse.
Definition: ZeroDofJoint.cpp:809
double getDampingCoefficient(std::size_t _index) const override
Get coefficient of joint damping (viscous friction) force.
Definition: ZeroDofJoint.cpp:647
bool hasPositionLimit(std::size_t _index) const override
Get whether the position of a generalized coordinate has limits.
Definition: ZeroDofJoint.cpp:243
void updateConstrainedTerms(double _timeStep) override
Update constrained terms for forward dynamics.
Definition: ZeroDofJoint.cpp:869
void setVelocityLowerLimits(const Eigen::VectorXd &lowerLimits) override
Set the velocity lower limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:323
double getForceUpperLimit(std::size_t _index) const override
Get upper limit for force.
Definition: ZeroDofJoint.cpp:542
void getInvMassMatrixSegment(Eigen::MatrixXd &_invMassMat, const std::size_t _col, const Eigen::Matrix6d &_artInertia, const Eigen::Vector6d &_spatialAcc) override
Definition: ZeroDofJoint.cpp:900
Eigen::VectorXd getAccelerationUpperLimits() const override
Get the acceleration upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:476
Eigen::VectorXd getForces() const override
Get the forces of all generalized coordinates in this Joint.
Definition: ZeroDofJoint.cpp:500
void updateTotalForce(const Eigen::Vector6d &_bodyForce, double _timeStep) override
Update joint total force.
Definition: ZeroDofJoint.cpp:802
void setInitialPosition(std::size_t _index, double _initial) override
Change the position that resetPositions() will give to this coordinate.
Definition: ZeroDofJoint.cpp:261
DegreeOfFreedom * getDof(std::size_t) override
Get an object to access the _index-th degree of freedom (generalized coordinate) of this Joint.
Definition: ZeroDofJoint.cpp:63
double getPositionUpperLimit(std::size_t index) const override
Get upper limit for position.
Definition: ZeroDofJoint.cpp:224
void setDampingCoefficient(std::size_t _index, double _d) override
Set coefficient of joint damping (viscous friction) force.
Definition: ZeroDofJoint.cpp:641
void setForceLowerLimit(std::size_t _index, double _force) override
Set lower limit for force.
Definition: ZeroDofJoint.cpp:512
Eigen::VectorXd getVelocityUpperLimits() const override
Get the velocity upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:356
void setVelocities(const Eigen::VectorXd &_velocities) override
Set the velocities of all generalized coordinates in this Joint.
Definition: ZeroDofJoint.cpp:298
Eigen::VectorXd getPositionUpperLimits() const override
Get the position upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:237
void updateDegreeOfFreedomNames() override
Update the names of the joint's degrees of freedom.
Definition: ZeroDofJoint.cpp:684
double getVelocityUpperLimit(std::size_t _index) const override
Get upper limit for velocity.
Definition: ZeroDofJoint.cpp:343
double getInitialPosition(std::size_t _index) const override
Get the position that resetPosition() will give to this coordinate.
Definition: ZeroDofJoint.cpp:268
void updateTotalForceForInvMassMatrix(const Eigen::Vector6d &_bodyForce) override
Definition: ZeroDofJoint.cpp:893
double getCoulombFriction(std::size_t _index) const override
Get joint Coulomb friction froce.
Definition: ZeroDofJoint.cpp:660
void updateInvProjArtInertia(const Eigen::Matrix6d &_artInertia) override
Update inverse of projected articulated body inertia.
Definition: ZeroDofJoint.cpp:762
void setVelocityUpperLimits(const Eigen::VectorXd &upperLimits) override
Set the velocity upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:349
void resetTotalImpulses() override
Set total impulses to zero.
Definition: ZeroDofJoint.cpp:815
void resetVelocity(std::size_t _index) override
Set the velocity of a generalized coordinate in this Joint to its initial velocity.
Definition: ZeroDofJoint.cpp:362
void setAccelerationLowerLimit(std::size_t _index, double _acceleration) override
Set lower limit for acceleration.
Definition: ZeroDofJoint.cpp:430
Eigen::VectorXd getInitialPositions() const override
Get the positions that resetPositions() will give to this Joint's coordinates.
Definition: ZeroDofJoint.cpp:280
void integratePositions(double _dt) override
Integrate positions using Euler method.
Definition: ZeroDofJoint.cpp:598
void setVelocity(std::size_t _index, double _velocity) override
Set the velocity of a single generalized coordinate.
Definition: ZeroDofJoint.cpp:286
ZeroDofJoint(const ZeroDofJoint &)=delete
void resetAccelerations() override
Set the accelerations of all generalized coordinates in this Joint to zero.
Definition: ZeroDofJoint.cpp:424
virtual ~ZeroDofJoint()
Destructor.
Definition: ZeroDofJoint.cpp:51
void setPositions(const Eigen::VectorXd &_positions) override
Set the positions of all generalized coordinates in this Joint.
Definition: ZeroDofJoint.cpp:179
void setForceUpperLimit(std::size_t _index, double _force) override
Set upper limit for force.
Definition: ZeroDofJoint.cpp:536
double getAcceleration(std::size_t _index) const override
Get the acceleration of a single generalized coordinate.
Definition: ZeroDofJoint.cpp:406
double getPosition(std::size_t _index) const override
Get the position of a single generalized coordinate.
Definition: ZeroDofJoint.cpp:171
Eigen::VectorXd getVelocityLowerLimits() const override
Get the velocity lower limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:330
const math::Jacobian getRelativeJacobianTimeDeriv() const override
Get time derivative of spatial Jacobian of the child BodyNode relative to the parent BodyNode express...
Definition: ZeroDofJoint.cpp:710
Eigen::VectorXd getInitialVelocities() const override
Get the velocities that resetVelocities() will give to this Joint's coordinates.
Definition: ZeroDofJoint.cpp:393
Eigen::VectorXd getPositionDifferences(const Eigen::VectorXd &_q2, const Eigen::VectorXd &_q1) const override
Return the difference of two generalized coordinates which are measured in the configuration space of...
Definition: ZeroDofJoint.cpp:610
double getAccelerationUpperLimit(std::size_t _index) const override
Get upper limit for acceleration.
Definition: ZeroDofJoint.cpp:463
void setAccelerationLowerLimits(const Eigen::VectorXd &lowerLimits) override
Set the acceleration upper limits of all the generalized coordinates.
Definition: ZeroDofJoint.cpp:443
void updateAcceleration(const Eigen::Matrix6d &_artInertia, const Eigen::Vector6d &_spatialAcc) override
Update joint acceleration.
Definition: ZeroDofJoint.cpp:821
void setPositionUpperLimit(std::size_t index, double position) override
Set upper limit for position.
Definition: ZeroDofJoint.cpp:217
void setPositionLowerLimit(std::size_t _index, double _position) override
Set lower limit for position.
Definition: ZeroDofJoint.cpp:191
void setAccelerations(const Eigen::VectorXd &_accelerations) override
Set the accelerations of all generalized coordinates in this Joint.
Definition: ZeroDofJoint.cpp:412
void setForces(const Eigen::VectorXd &_forces) override
Set the forces of all generalized coordinates in this Joint.
Definition: ZeroDofJoint.cpp:494
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: ZeroDofJoint.hpp:51
virtual ~Properties()=default
Properties(const Joint::Properties &_properties=Joint::Properties())
Definition: ZeroDofJoint.cpp:44
Definition: JointAspect.hpp:112