dartpy 7.0.0.dev0__cp312-cp312-manylinux_2_39_x86_64.whl

include/dart/dynamics/PointMass.hpp

@@ -0,0 +1,709 @@
+/*
+ * Copyright (c) 2011-2025, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/main/LICENSE
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DART_DYNAMICS_POINTMASS_HPP_
+#define DART_DYNAMICS_POINTMASS_HPP_
+
+#include <dart/dynamics/Entity.hpp>
+
+#include <dart/math/Helpers.hpp>
+
+#include <Eigen/Dense>
+
+#include <vector>
+
+namespace dart {
+namespace dynamics {
+
+class EllipsoidShape;
+class SoftBodyNode;
+
+class PointMassNotifier;
+
+///
+class PointMass : public common::Subject
+{
+public:
+  friend class SoftBodyNode;
+
+  /// State for each PointMass
+  struct State
+  {
+    /// Position
+    Eigen::Vector3d mPositions;
+
+    /// Generalized velocity
+    Eigen::Vector3d mVelocities;
+
+    /// Generalized acceleration
+    Eigen::Vector3d mAccelerations;
+
+    /// Generalized force
+    Eigen::Vector3d mForces;
+
+    /// Default constructor
+    State(
+        const Eigen::Vector3d& positions = Eigen::Vector3d::Zero(),
+        const Eigen::Vector3d& velocities = Eigen::Vector3d::Zero(),
+        const Eigen::Vector3d& accelerations = Eigen::Vector3d::Zero(),
+        const Eigen::Vector3d& forces = Eigen::Vector3d::Zero());
+
+    bool operator==(const State& other) const;
+
+    virtual ~State() = default;
+  };
+
+  /// Properties for each PointMass
+  struct Properties
+  {
+    /// Resting position viewed in the parent SoftBodyNode frame
+    Eigen::Vector3d mX0;
+
+    /// Mass.
+    double mMass;
+
+    /// Indices of connected Point Masses
+    std::vector<std::size_t> mConnectedPointMassIndices;
+
+    /// Lower limit of position
+    Eigen::Vector3d mPositionLowerLimits; // Currently unused
+
+    /// Upper limit of position
+    Eigen::Vector3d mPositionUpperLimits; // Currently unused
+
+    /// Min value allowed.
+    Eigen::Vector3d mVelocityLowerLimits; // Currently unused
+
+    /// Max value allowed.
+    Eigen::Vector3d mVelocityUpperLimits; // Currently unused
+
+    /// Min value allowed.
+    Eigen::Vector3d mAccelerationLowerLimits; // Currently unused
+
+    /// upper limit of generalized acceleration
+    Eigen::Vector3d mAccelerationUpperLimits; // Currently unused
+
+    /// Min value allowed.
+    Eigen::Vector3d mForceLowerLimits; // Currently unused
+
+    /// Max value allowed.
+    Eigen::Vector3d mForceUpperLimits; // Currently unused
+
+    Properties(
+        const Eigen::Vector3d& _X0 = Eigen::Vector3d::Zero(),
+        double _mass = 0.0005,
+        const std::vector<std::size_t>& _connections
+        = std::vector<std::size_t>(),
+        const Eigen::Vector3d& _positionLowerLimits
+        = Eigen::Vector3d::Constant(-math::constantsd::inf()),
+        const Eigen::Vector3d& _positionUpperLimits
+        = Eigen::Vector3d::Constant(math::constantsd::inf()),
+        const Eigen::Vector3d& _velocityLowerLimits
+        = Eigen::Vector3d::Constant(-math::constantsd::inf()),
+        const Eigen::Vector3d& _velocityUpperLimits
+        = Eigen::Vector3d::Constant(math::constantsd::inf()),
+        const Eigen::Vector3d& _accelerationLowerLimits
+        = Eigen::Vector3d::Constant(-math::constantsd::inf()),
+        const Eigen::Vector3d& _accelerationUpperLimits
+        = Eigen::Vector3d::Constant(math::constantsd::inf()),
+        const Eigen::Vector3d& _forceLowerLimits
+        = Eigen::Vector3d::Constant(-math::constantsd::inf()),
+        const Eigen::Vector3d& _forceUpperLimits
+        = Eigen::Vector3d::Constant(math::constantsd::inf()));
+
+    void setRestingPosition(const Eigen::Vector3d& _x);
+
+    void setMass(double _mass);
+
+    bool operator==(const Properties& other) const;
+
+    bool operator!=(const Properties& other) const;
+
+    virtual ~Properties() = default;
+  };
+
+  //--------------------------------------------------------------------------
+  // Constructor and Desctructor
+  //--------------------------------------------------------------------------
+
+  /// Default destructor
+  virtual ~PointMass();
+
+  /// State of this PointMass
+  State& getState();
+
+  /// State of this PointMass
+  const State& getState() const;
+
+  ///
+  std::size_t getIndexInSoftBodyNode() const;
+
+  ///
+  void setMass(double _mass);
+
+  ///
+  double getMass() const;
+
+  ///
+  double getPsi() const;
+
+  ///
+  double getImplicitPsi() const;
+
+  ///
+  double getPi() const;
+
+  ///
+  double getImplicitPi() const;
+
+  ///
+  void addConnectedPointMass(PointMass* _pointMass);
+
+  ///
+  std::size_t getNumConnectedPointMasses() const;
+
+  ///
+  PointMass* getConnectedPointMass(std::size_t _idx);
+
+  ///
+  const PointMass* getConnectedPointMass(std::size_t _idx) const;
+
+  /// Set whether this point mass is colliding with other objects. Note that
+  /// this status is set by the constraint solver during dynamics simulation but
+  /// not by collision detector.
+  /// \param[in] _isColliding True if this point mass is colliding.
+  void setColliding(bool _isColliding);
+
+  /// Return whether this point mass is set to be colliding with other objects.
+  /// \return True if this point mass is colliding.
+  bool isColliding();
+
+  //----------------------------------------------------------------------------
+
+  // Documentation inherited
+  std::size_t getNumDofs() const;
+
+  //  // Documentation inherited
+  //  void setIndexInSkeleton(std::size_t _index, std::size_t _indexInSkeleton);
+
+  //  // Documentation inherited
+  //  std::size_t getIndexInSkeleton(std::size_t _index) const;
+
+  //----------------------------------------------------------------------------
+  // Position
+  //----------------------------------------------------------------------------
+
+  // Documentation inherited
+  void setPosition(std::size_t _index, double _position);
+
+  // Documentation inherited
+  double getPosition(std::size_t _index) const;
+
+  // Documentation inherited
+  void setPositions(const Eigen::Vector3d& _positions);
+
+  // Documentation inherited
+  const Eigen::Vector3d& getPositions() const;
+
+  // Documentation inherited
+  void resetPositions();
+
+  //----------------------------------------------------------------------------
+  // Velocity
+  //----------------------------------------------------------------------------
+
+  // Documentation inherited
+  void setVelocity(std::size_t _index, double _velocity);
+
+  // Documentation inherited
+  double getVelocity(std::size_t _index) const;
+
+  // Documentation inherited
+  void setVelocities(const Eigen::Vector3d& _velocities);
+
+  // Documentation inherited
+  const Eigen::Vector3d& getVelocities() const;
+
+  // Documentation inherited
+  void resetVelocities();
+
+  //----------------------------------------------------------------------------
+  // Acceleration
+  //----------------------------------------------------------------------------
+
+  // Documentation inherited
+  void setAcceleration(std::size_t _index, double _acceleration);
+
+  // Documentation inherited
+  double getAcceleration(std::size_t _index) const;
+
+  // Documentation inherited
+  void setAccelerations(const Eigen::Vector3d& _accelerations);
+
+  // Documentation inherited
+  const Eigen::Vector3d& getAccelerations() const;
+
+  /// Get the Eta term of this PointMass
+  const Eigen::Vector3d& getPartialAccelerations() const;
+
+  // Documentation inherited
+  void resetAccelerations();
+
+  //----------------------------------------------------------------------------
+  // Force
+  //----------------------------------------------------------------------------
+
+  // Documentation inherited
+  void setForce(std::size_t _index, double _force);
+
+  // Documentation inherited
+  double getForce(std::size_t _index);
+
+  // Documentation inherited
+  void setForces(const Eigen::Vector3d& _forces);
+
+  // Documentation inherited
+  const Eigen::Vector3d& getForces() const;
+
+  // Documentation inherited
+  void resetForces();
+
+  //----------------------------------------------------------------------------
+  // Velocity change
+  //----------------------------------------------------------------------------
+
+  // Documentation inherited
+  void setVelocityChange(std::size_t _index, double _velocityChange);
+
+  // Documentation inherited
+  double getVelocityChange(std::size_t _index);
+
+  // Documentation inherited
+  void resetVelocityChanges();
+
+  //----------------------------------------------------------------------------
+  // Constraint impulse
+  //----------------------------------------------------------------------------
+
+  // Documentation inherited
+  void setConstraintImpulse(std::size_t _index, double _impulse);
+
+  // Documentation inherited
+  double getConstraintImpulse(std::size_t _index);
+
+  // Documentation inherited
+  void resetConstraintImpulses();
+
+  //----------------------------------------------------------------------------
+  // Integration
+  //----------------------------------------------------------------------------
+
+  // Documentation inherited
+  void integratePositions(double _dt);
+
+  // Documentation inherited
+  void integrateVelocities(double _dt);
+
+  //----------------------------------------------------------------------------
+
+  /// Add linear Cartesian force to this node.
+  /// \param[in] _force External force.
+  /// \param[in] _isForceLocal True if _force's reference frame is of the parent
+  ///                          soft body node. False if _force's reference frame
+  ///                          is of the world.
+  void addExtForce(const Eigen::Vector3d& _force, bool _isForceLocal = false);
+
+  ///
+  void clearExtForce();
+
+  //----------------------------------------------------------------------------
+  // Constraints
+  //   - Following functions are managed by constraint solver.
+  //----------------------------------------------------------------------------
+  /// Set constraint impulse
+  void setConstraintImpulse(
+      const Eigen::Vector3d& _constImp, bool _isLocal = false);
+
+  /// Add constraint impulse
+  void addConstraintImpulse(
+      const Eigen::Vector3d& _constImp, bool _isLocal = false);
+
+  /// Clear constraint impulse
+  void clearConstraintImpulse();
+
+  /// Get constraint impulse
+  Eigen::Vector3d getConstraintImpulses() const;
+
+  //----------------------------------------------------------------------------
+  ///
+  void setRestingPosition(const Eigen::Vector3d& _p);
+
+  ///
+  const Eigen::Vector3d& getRestingPosition() const;
+
+  ///
+  const Eigen::Vector3d& getLocalPosition() const;
+
+  ///
+  const Eigen::Vector3d& getWorldPosition() const;
+
+  /// \todo Temporary function.
+  Eigen::Matrix<double, 3, Eigen::Dynamic> getBodyJacobian();
+  Eigen::Matrix<double, 3, Eigen::Dynamic> getWorldJacobian();
+
+  /// Return velocity change due to impulse
+  const Eigen::Vector3d& getBodyVelocityChange() const;
+
+  ///
+  SoftBodyNode* getParentSoftBodyNode();
+
+  ///
+  const SoftBodyNode* getParentSoftBodyNode() const;
+
+  /// The number of the generalized coordinates by which this node is
+  ///        affected.
+  //  int getNumDependentGenCoords() const;
+
+  /// Return a generalized coordinate index from the array index
+  ///        (< getNumDependentDofs).
+  //  int getDependentGenCoord(int _arrayIndex) const;
+
+  /// Get the generalized velocity at the position of this point mass
+  ///        where the velocity is expressed in the parent soft body node frame.
+  const Eigen::Vector3d& getBodyVelocity() const;
+
+  /// Get the generalized velocity at the position of this point mass
+  ///        where the velocity is expressed in the world frame.
+  Eigen::Vector3d getWorldVelocity() const;
+
+  /// Get the generalized acceleration at the position of this point mass
+  ///        where the acceleration is expressed in the parent soft body node
+  ///        frame.
+  const Eigen::Vector3d& getBodyAcceleration() const;
+
+  /// Get the generalized acceleration at the position of this point mass
+  ///        where the acceleration is expressed in the world frame.
+  Eigen::Vector3d getWorldAcceleration() const;
+
+protected:
+  /// Constructor used by SoftBodyNode
+  explicit PointMass(SoftBodyNode* _softBodyNode);
+
+  ///
+  void init();
+
+  //----------------------------------------------------------------------------
+  /// \{ \name Recursive dynamics routines
+  //----------------------------------------------------------------------------
+
+  /// \brief Update transformation.
+  void updateTransform() const;
+
+  /// \brief Update body velocity.
+  void updateVelocity() const;
+
+  /// \brief Update partial body acceleration due to parent joint's velocity.
+  void updatePartialAcceleration() const;
+
+  /// \brief Update articulated body inertia. Forward dynamics routine.
+  /// \param[in] _timeStep Rquired for implicit joint stiffness and damping.
+  void updateArtInertiaFD(double _timeStep) const;
+
+  /// \brief Update bias force associated with the articulated body inertia.
+  /// Forward dynamics routine.
+  /// \param[in] _dt Required for implicit joint stiffness and damping.
+  /// \param[in] _gravity Vector of gravitational acceleration
+  void updateBiasForceFD(double _dt, const Eigen::Vector3d& _gravity);
+
+  /// \brief Update bias impulse associated with the articulated body inertia.
+  /// Impulse-based forward dynamics routine.
+  void updateBiasImpulseFD();
+
+  /// \brief Update body acceleration with the partial body acceleration.
+  void updateAccelerationID() const;
+
+  /// \brief Update body acceleration. Forward dynamics routine.
+  void updateAccelerationFD();
+
+  /// \brief Update body velocity change. Impluse-based forward dynamics
+  /// routine.
+  void updateVelocityChangeFD();
+
+  /// \brief Update body force. Inverse dynamics routine.
+  void updateTransmittedForceID(
+      const Eigen::Vector3d& _gravity, bool _withExternalForces = false);
+
+  /// \brief Update body force. Forward dynamics routine.
+  void updateTransmittedForce();
+
+  /// \brief Update body force. Impulse-based forward dynamics routine.
+  void updateTransmittedImpulse();
+
+  /// \brief Update the joint force. Inverse dynamics routine.
+  void updateJointForceID(
+      double _timeStep, double _withDampingForces, double _withSpringForces);
+
+  /// \brief Update constrained terms due to the constraint impulses. Foward
+  /// dynamics routine.
+  void updateConstrainedTermsFD(double _timeStep);
+
+  /// \}
+
+  //----------------------------------------------------------------------------
+  /// \{ \name Equations of motion related routines
+  //----------------------------------------------------------------------------
+
+  ///
+  void updateMassMatrix();
+
+  ///
+  void aggregateMassMatrix(Eigen::MatrixXd& _MCol, int _col);
+
+  ///
+  void aggregateAugMassMatrix(
+      Eigen::MatrixXd& _MCol, int _col, double _timeStep);
+
+  ///
+  void updateInvMassMatrix();
+
+  ///
+  void updateInvAugMassMatrix();
+
+  ///
+  void aggregateInvMassMatrix(Eigen::MatrixXd& _MInvCol, int _col);
+
+  ///
+  void aggregateInvAugMassMatrix(
+      Eigen::MatrixXd& _MInvCol, int _col, double _timeStep);
+
+  ///
+  void aggregateGravityForceVector(
+      Eigen::VectorXd& _g, const Eigen::Vector3d& _gravity);
+
+  ///
+  void updateCombinedVector();
+
+  ///
+  void aggregateCombinedVector(
+      Eigen::VectorXd& _Cg, const Eigen::Vector3d& _gravity);
+
+  /// Aggregate the external forces mFext in the generalized
+  ///        coordinates recursively.
+  void aggregateExternalForces(Eigen::VectorXd& _Fext);
+
+  /// \}
+
+  //-------------------- Cache Data for Mass Matrix ----------------------------
+  ///
+  Eigen::Vector3d mM_dV;
+
+  ///
+  Eigen::Vector3d mM_F;
+
+  //----------------- Cache Data for Mass Inverse Matrix -----------------------
+  ///
+  Eigen::Vector3d mBiasForceForInvMeta;
+
+  //---------------- Cache Data for Gravity Force Vector -----------------------
+  ///
+  Eigen::Vector3d mG_F;
+
+  //------------------- Cache Data for Combined Vector -------------------------
+  ///
+  Eigen::Vector3d mCg_dV;
+
+  ///
+  Eigen::Vector3d mCg_F;
+
+protected:
+  // TODO(JS): Need?
+  ///
+  //  Eigen::Matrix<std::size_t, 3, 1> mIndexInSkeleton;
+
+  /// SoftBodyNode that this PointMass belongs to
+  SoftBodyNode* mParentSoftBodyNode;
+
+  /// Index of this PointMass within the SoftBodyNode
+  std::size_t mIndex;
+
+  //----------------------------------------------------------------------------
+  // Configuration
+  //----------------------------------------------------------------------------
+
+  /// Derivatives w.r.t. an arbitrary scalr variable
+  Eigen::Vector3d mPositionDeriv;
+
+  //----------------------------------------------------------------------------
+  // Velocity
+  //----------------------------------------------------------------------------
+
+  /// Derivatives w.r.t. an arbitrary scalr variable
+  Eigen::Vector3d mVelocitiesDeriv;
+
+  //----------------------------------------------------------------------------
+  // Acceleration
+  //----------------------------------------------------------------------------
+
+  /// Derivatives w.r.t. an arbitrary scalr variable
+  Eigen::Vector3d mAccelerationsDeriv;
+
+  //----------------------------------------------------------------------------
+  // Force
+  //----------------------------------------------------------------------------
+
+  /// Derivatives w.r.t. an arbitrary scalr variable
+  Eigen::Vector3d mForcesDeriv;
+
+  //----------------------------------------------------------------------------
+  // Impulse
+  //----------------------------------------------------------------------------
+
+  /// Change of generalized velocity
+  Eigen::Vector3d mVelocityChanges;
+
+  //  /// Generalized impulse
+  //  Eigen::Vector3d mImpulse;
+
+  /// Generalized constraint impulse
+  Eigen::Vector3d mConstraintImpulses;
+
+  //----------------------------------------------------------------------------
+
+  /// Current position viewed in world frame.
+  mutable Eigen::Vector3d mW;
+
+  /// Current position viewed in parent soft body node frame.
+  mutable Eigen::Vector3d mX;
+
+  /// Current velocity viewed in parent soft body node frame.
+  mutable Eigen::Vector3d mV;
+
+  /// Partial Acceleration of this PointMass
+  mutable Eigen::Vector3d mEta;
+
+  ///
+  Eigen::Vector3d mAlpha;
+
+  ///
+  Eigen::Vector3d mBeta;
+
+  /// Current acceleration viewed in parent body node frame.
+  mutable Eigen::Vector3d mA;
+
+  ///
+  Eigen::Vector3d mF;
+
+  ///
+  mutable double mPsi;
+
+  ///
+  mutable double mImplicitPsi;
+
+  ///
+  mutable double mPi;
+
+  ///
+  mutable double mImplicitPi;
+
+  /// Bias force
+  Eigen::Vector3d mB;
+
+  /// External force.
+  Eigen::Vector3d mFext;
+
+  /// A increasingly sorted list of dependent dof indices.
+  std::vector<std::size_t> mDependentGenCoordIndices;
+
+  /// Whether the node is currently in collision with another node.
+  bool mIsColliding;
+
+  //------------------------- Impulse-based Dyanmics ---------------------------
+  /// Velocity change due to constraint impulse
+  Eigen::Vector3d mDelV;
+
+  /// Impulsive bias force due to external impulsive force exerted on
+  ///        bodies of the parent skeleton.
+  Eigen::Vector3d mImpB;
+
+  /// Cache data for mImpB
+  Eigen::Vector3d mImpAlpha;
+
+  /// Cache data for mImpB
+  Eigen::Vector3d mImpBeta;
+
+  /// Generalized impulsive body force w.r.t. body frame.
+  Eigen::Vector3d mImpF;
+
+  PointMassNotifier* mNotifier;
+};
+
+// struct PointMassPair
+//{
+//  PointMass* pm1;
+//  PointMass* pm2;
+//};
+
+class PointMassNotifier : public Entity
+{
+public:
+  PointMassNotifier(SoftBodyNode* _parentSoftBody, const std::string& _name);
+
+  bool needsPartialAccelerationUpdate() const;
+
+  void clearTransformNotice();
+  void clearVelocityNotice();
+  void clearPartialAccelerationNotice();
+  void clearAccelerationNotice();
+
+  void dirtyTransform() override;
+  void dirtyVelocity() override;
+  void dirtyAcceleration() override;
+
+  // Documentation inherited
+  const std::string& setName(const std::string& _name) override;
+
+  // Documentation inherited
+  const std::string& getName() const override;
+
+protected:
+  std::string mName;
+
+  bool mNeedPartialAccelerationUpdate;
+  // TODO(JS): Rename this to mIsPartialAccelerationDirty in DART 7
+
+  SoftBodyNode* mParentSoftBodyNode;
+};
+
+} // namespace dynamics
+} // namespace dart
+
+#endif // DART_DYNAMICS_POINTMASS_HPP_