dartpy 7.0.0.dev0__cp313-cp313-manylinux_2_39_x86_64.whl

include/dart/constraint/PgsBoxedLcpSolver.hpp

@@ -0,0 +1,106 @@
+/*
+ * 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_CONSTRAINT_PGSBOXEDLCPSOLVER_HPP_
+#define DART_CONSTRAINT_PGSBOXEDLCPSOLVER_HPP_
+
+#include <dart/constraint/BoxedLcpSolver.hpp>
+
+#include <vector>
+
+namespace dart {
+namespace constraint {
+
+/// Implementation of projected Gauss-Seidel (PGS) LCP solver.
+class PgsBoxedLcpSolver : public BoxedLcpSolver
+{
+public:
+  struct Option
+  {
+    int mMaxIteration;
+    double mDeltaXThreshold;
+    double mRelativeDeltaXTolerance;
+    double mEpsilonForDivision;
+    bool mRandomizeConstraintOrder;
+
+    Option(
+        int maxIteration = 30,
+        double deltaXTolerance = 1e-6,
+        double relativeDeltaXTolerance = 1e-3,
+        double epsilonForDivision = 1e-9,
+        bool randomizeConstraintOrder = false);
+  };
+
+  // Documentation inherited.
+  const std::string& getType() const override;
+
+  /// Returns type for this class
+  static const std::string& getStaticType();
+
+  // Documentation inherited.
+  bool solve(
+      int n,
+      double* A,
+      double* x,
+      double* b,
+      int nub,
+      double* lo,
+      double* hi,
+      int* findex,
+      bool earlyTermination) override;
+
+#if DART_BUILD_MODE_DEBUG
+  // Documentation inherited.
+  bool canSolve(int n, const double* A) override;
+#endif
+
+  /// Sets options
+  void setOption(const Option& option);
+
+  /// Returns options.
+  const Option& getOption() const;
+
+protected:
+  Option mOption;
+
+  mutable std::vector<int> mCacheOrder;
+  mutable std::vector<double> mCacheD;
+  mutable Eigen::VectorXd mCachedNormalizedA;
+  mutable Eigen::MatrixXd mCachedNormalizedB;
+  mutable Eigen::VectorXd mCacheZ;
+  mutable Eigen::VectorXd mCacheOldX;
+};
+
+} // namespace constraint
+} // namespace dart
+
+#endif // DART_CONSTRAINT_PGSBOXEDLCPSOLVER_HPP_

include/dart/constraint/ServoMotorConstraint.hpp

@@ -0,0 +1,151 @@
+/*
+ * 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_CONSTRAINT_SERVOMOTORCONSTRAINT_HPP_
+#define DART_CONSTRAINT_SERVOMOTORCONSTRAINT_HPP_
+
+#include <dart/constraint/ConstraintBase.hpp>
+
+namespace dart {
+
+namespace dynamics {
+class BodyNode;
+class Joint;
+} // namespace dynamics
+
+namespace constraint {
+
+/// Servo motor constraint
+class ServoMotorConstraint : public ConstraintBase
+{
+public:
+  /// Constructor
+  explicit ServoMotorConstraint(dynamics::Joint* joint);
+
+  /// Destructor
+  virtual ~ServoMotorConstraint();
+
+  // Documentation inherited
+  const std::string& getType() const override;
+
+  /// Returns constraint type for this class.
+  static const std::string& getStaticType();
+
+  //----------------------------------------------------------------------------
+  // Property settings
+  //----------------------------------------------------------------------------
+
+  /// Set global constraint force mixing parameter
+  static void setConstraintForceMixing(double cfm);
+
+  /// Get global constraint force mixing parameter
+  static double getConstraintForceMixing();
+
+  //----------------------------------------------------------------------------
+  // Friendship
+  //----------------------------------------------------------------------------
+
+  friend class ConstraintSolver;
+  friend class ConstrainedGroup;
+
+protected:
+  //----------------------------------------------------------------------------
+  // Constraint virtual functions
+  //----------------------------------------------------------------------------
+
+  // Documentation inherited
+  void update() override;
+
+  // Documentation inherited
+  void getInformation(ConstraintInfo* lcp) override;
+
+  // Documentation inherited
+  void applyUnitImpulse(std::size_t index) override;
+
+  // Documentation inherited
+  void getVelocityChange(double* delVel, bool withCfm) override;
+
+  // Documentation inherited
+  void excite() override;
+
+  // Documentation inherited
+  void unexcite() override;
+
+  // Documentation inherited
+  void applyImpulse(double* lambda) override;
+
+  // Documentation inherited
+  dynamics::SkeletonPtr getRootSkeleton() const override;
+
+  // Documentation inherited
+  bool isActive() const override;
+
+private:
+  ///
+  dynamics::Joint* mJoint;
+
+  ///
+  dynamics::BodyNode* mBodyNode;
+
+  /// Index of applied impulse
+  std::size_t mAppliedImpulseIndex;
+
+  ///
+  std::size_t mLifeTime[6];
+  // TODO(JS): Lifetime should be considered only when we use iterative lcp
+  // solver
+
+  ///
+  bool mActive[6];
+
+  ///
+  double mNegativeVelocityError[6];
+
+  ///
+  double mOldX[6];
+
+  ///
+  double mUpperBound[6];
+
+  ///
+  double mLowerBound[6];
+
+  /// Global constraint force mixing parameter in the range of [1e-9, 1]. The
+  /// default is 1e-5
+  /// \sa http://www.ode.org/ode-latest-userguide.html#sec_3_8_0
+  static double mConstraintForceMixing;
+};
+
+} // namespace constraint
+} // namespace dart
+
+#endif // DART_CONSTRAINT_SERVOMOTORCONSTRAINT_HPP_

include/dart/constraint/SmartPointer.hpp

@@ -0,0 +1,69 @@
+/*
+ * 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_CONSTRAINT_SMARTPOINTER_HPP_
+#define DART_CONSTRAINT_SMARTPOINTER_HPP_
+
+#include <dart/common/SmartPointer.hpp>
+
+namespace dart {
+namespace constraint {
+
+DART_COMMON_DECLARE_SMART_POINTERS(ConstraintSolver)
+
+DART_COMMON_DECLARE_SHARED_WEAK(ConstrainedGroup)
+
+DART_COMMON_DECLARE_SHARED_WEAK(ConstraintBase)
+DART_COMMON_DECLARE_SHARED_WEAK(ClosedLoopConstraint)
+DART_COMMON_DECLARE_SHARED_WEAK(ContactConstraint)
+DART_COMMON_DECLARE_SHARED_WEAK(ContactSurfaceHandler)
+DART_COMMON_DECLARE_SHARED_WEAK(SoftContactConstraint)
+DART_COMMON_DECLARE_SHARED_WEAK(JointConstraint)
+DART_COMMON_DECLARE_SHARED_WEAK(MimicMotorConstraint)
+DART_COMMON_DECLARE_SHARED_WEAK(JointCoulombFrictionConstraint)
+
+DART_COMMON_DECLARE_SHARED_WEAK(LCPSolver)
+DART_COMMON_DECLARE_SHARED_WEAK(BoxedLcpSolver)
+DART_COMMON_DECLARE_SHARED_WEAK(PgsBoxedLcpSolver)
+DART_COMMON_DECLARE_SHARED_WEAK(PsorBoxedLcpSolver)
+DART_COMMON_DECLARE_SHARED_WEAK(JacobiBoxedLcpSolver)
+
+DART_COMMON_DECLARE_SHARED_WEAK(DynamicJointConstraint)
+DART_COMMON_DECLARE_SHARED_WEAK(BallJointConstraint)
+DART_COMMON_DECLARE_SHARED_WEAK(WeldJointConstraint)
+
+DART_COMMON_DECLARE_SHARED_WEAK(BalanceConstraint)
+
+} // namespace constraint
+} // namespace dart
+
+#endif // DART_CONSTRAINT_SMARTPOINTER_HPP_

include/dart/constraint/SoftContactConstraint.hpp

@@ -0,0 +1,262 @@
+/*
+ * 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_CONSTRAINT_SOFTCONTACTCONSTRAINT_HPP_
+#define DART_CONSTRAINT_SOFTCONTACTCONSTRAINT_HPP_
+
+#include <dart/constraint/ConstraintBase.hpp>
+
+#include <dart/collision/CollisionDetector.hpp>
+
+#include <dart/math/MathTypes.hpp>
+
+namespace dart {
+
+namespace collision {
+class SoftCollisionInfo;
+} // namespace collision
+
+namespace dynamics {
+class BodyNode;
+class SoftBodyNode;
+class PointMass;
+class Skeleton;
+} // namespace dynamics
+
+namespace constraint {
+
+/// SoftContactConstraint represents a contact constraint between two bodies
+class SoftContactConstraint : public ConstraintBase
+{
+public:
+  /// Constructor
+  SoftContactConstraint(collision::Contact& _contact, double _timeStep);
+
+  /// Destructor
+  virtual ~SoftContactConstraint();
+
+  // Documentation inherited
+  const std::string& getType() const override;
+
+  /// Returns constraint type for this class.
+  static const std::string& getStaticType();
+
+  //----------------------------------------------------------------------------
+  // Property settings
+  //----------------------------------------------------------------------------
+
+  /// Set global error reduction parameter
+  static void setErrorAllowance(double _allowance);
+
+  /// Get global error reduction parameter
+  static double getErrorAllowance();
+
+  /// Set global error reduction parameter
+  static void setErrorReductionParameter(double _erp);
+
+  /// Get global error reduction parameter
+  static double getErrorReductionParameter();
+
+  /// Set global error reduction parameter
+  static void setMaxErrorReductionVelocity(double _erv);
+
+  /// Get global error reduction parameter
+  static double getMaxErrorReductionVelocity();
+
+  /// Set global constraint force mixing parameter
+  static void setConstraintForceMixing(double _cfm);
+
+  /// Get global constraint force mixing parameter
+  static double getConstraintForceMixing();
+
+  /// Set first frictional direction
+  void setFrictionDirection(const Eigen::Vector3d& _dir);
+
+  /// Get first frictional direction
+  const Eigen::Vector3d& getFrictionDirection1() const;
+
+  //----------------------------------------------------------------------------
+  // Friendship
+  //----------------------------------------------------------------------------
+
+  friend class ConstraintSolver;
+  friend class ConstrainedGroup;
+
+protected:
+  //----------------------------------------------------------------------------
+  // Constraint virtual functions
+  //----------------------------------------------------------------------------
+
+  // Documentation inherited
+  void update() override;
+
+  // Documentation inherited
+  void getInformation(ConstraintInfo* _info) override;
+
+  // Documentation inherited
+  void applyUnitImpulse(std::size_t _idx) override;
+
+  // Documentation inherited
+  void getVelocityChange(double* _vel, bool _withCfm) override;
+
+  // Documentation inherited
+  void excite() override;
+
+  // Documentation inherited
+  void unexcite() override;
+
+  // Documentation inherited
+  void applyImpulse(double* _lambda) override;
+
+  // Documentation inherited
+  dynamics::SkeletonPtr getRootSkeleton() const override;
+
+  // Documentation inherited
+  void uniteSkeletons() override;
+
+  // Documentation inherited
+  bool isActive() const override;
+
+  static double computeFrictionCoefficient(
+      const dynamics::ShapeNode* shapeNode);
+  static double computeRestitutionCoefficient(
+      const dynamics::ShapeNode* shapeNode);
+
+private:
+  /// Get change in relative velocity at contact point due to external impulse
+  /// \param[out] _vel Change in relative velocity at contact point of the two
+  ///                  colliding bodies
+  void getRelVelocity(double* _vel);
+
+  ///
+  void updateFirstFrictionalDirection();
+
+  ///
+  Eigen::MatrixXd getTangentBasisMatrixODE(const Eigen::Vector3d& _n);
+
+  /// Find the nearest point mass from _point in a face, of which id is _faceId
+  /// in _softBodyNode.
+  dynamics::PointMass* selectCollidingPointMass(
+      dynamics::SoftBodyNode* _softBodyNode,
+      const Eigen::Vector3d& _point,
+      int _faceId) const;
+
+  /// Find the nearest point mass from _point in a face, of which id is _faceId
+  /// in _softBodyNode. Returns a pointer to a const, and is usable with a const
+  /// SoftBodyNode.
+  const dynamics::PointMass* selectCollidingPointMass(
+      const dynamics::SoftBodyNode* _softBodyNode,
+      const Eigen::Vector3d& _point,
+      int _faceId) const;
+
+private:
+  /// Time step
+  double mTimeStep;
+
+  /// Fircst body node
+  dynamics::BodyNode* mBodyNode1;
+
+  /// Second body node
+  dynamics::BodyNode* mBodyNode2;
+
+  /// First soft body node
+  dynamics::SoftBodyNode* mSoftBodyNode1;
+
+  /// Second soft body node
+  dynamics::SoftBodyNode* mSoftBodyNode2;
+
+  /// First point mass
+  dynamics::PointMass* mPointMass1;
+
+  /// Second point mass
+  dynamics::PointMass* mPointMass2;
+
+  // TODO(JS): For now, there is only one contact per contact constraint
+  /// Contacts between mBodyNode1 and mBodyNode2
+  std::vector<collision::Contact*> mContacts;
+
+  /// Soft collision information
+  collision::SoftCollisionInfo* mSoftCollInfo;
+
+  /// First frictional direction
+  Eigen::Vector3d mFirstFrictionalDirection;
+
+  /// Coefficient of Friction
+  double mFrictionCoeff;
+
+  /// Coefficient of restitution
+  double mRestitutionCoeff;
+
+  /// Local body jacobians for mBodyNode1
+  common::aligned_vector<Eigen::Vector6d> mJacobians1;
+
+  /// Local body jacobians for mBodyNode2
+  common::aligned_vector<Eigen::Vector6d> mJacobians2;
+
+  /// Contact normal expressed in body frame of the first body node
+  Eigen::Vector3d mBodyDirection1;
+
+  /// Contact normal expressed in body frame of the second body node
+  Eigen::Vector3d mBodyDirection2;
+
+  ///
+  bool mIsFrictionOn;
+
+  /// Index of applied impulse
+  std::size_t mAppliedImpulseIndex;
+
+  ///
+  bool mIsBounceOn;
+
+  ///
+  bool mActive;
+
+  /// Global constraint error allowance
+  static double mErrorAllowance;
+
+  /// Global constraint error redection parameter in the range of [0, 1]. The
+  /// default is 0.01.
+  static double mErrorReductionParameter;
+
+  /// Maximum error reduction velocity
+  static double mMaxErrorReductionVelocity;
+
+  /// Global constraint force mixing parameter in the range of [1e-9, 1]. The
+  /// default is 1e-5
+  /// \sa http://www.ode.org/ode-latest-userguide.html#sec_3_8_0
+  static double mConstraintForceMixing;
+};
+
+} // namespace constraint
+} // namespace dart
+
+#endif // DART_CONSTRAINT_SOFTCONTACTCONSTRAINT_HPP_