dartpy 7.0.0.dev0__cp312-cp312-manylinux_2_39_x86_64.whl

include/dart/constraint/JointCoulombFrictionConstraint.hpp

@@ -0,0 +1,149 @@
+/*
+ * 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_JOINTCOULOMBFRICTIONCONSTRAINT_HPP_
+#define DART_CONSTRAINT_JOINTCOULOMBFRICTIONCONSTRAINT_HPP_
+
+#include <dart/constraint/ConstraintBase.hpp>
+
+namespace dart {
+
+namespace dynamics {
+class BodyNode;
+class Joint;
+} // namespace dynamics
+
+namespace constraint {
+
+/// Joint Coulomb friction constraint
+class JointCoulombFrictionConstraint : public ConstraintBase
+{
+public:
+  /// Constructor
+  explicit JointCoulombFrictionConstraint(dynamics::Joint* _joint);
+
+  /// Destructor
+  virtual ~JointCoulombFrictionConstraint();
+
+  // 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];
+
+  ///
+  bool mActive[6];
+
+  ///
+  double mNegativeVel[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_JOINTCOULOMBFRICTIONCONSTRAINT_HPP_

include/dart/constraint/JointLimitConstraint.hpp

@@ -0,0 +1,185 @@
+/*
+ * 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_JOINTLIMITCONSTRAINT_HPP_
+#define DART_CONSTRAINT_JOINTLIMITCONSTRAINT_HPP_
+
+#include <dart/constraint/ConstraintBase.hpp>
+
+#include <Eigen/Dense>
+
+namespace dart {
+
+namespace dynamics {
+class BodyNode;
+class Joint;
+} // namespace dynamics
+
+namespace constraint {
+
+/// JointLimitConstraint handles joint position and velocity limits
+class JointLimitConstraint : public ConstraintBase
+{
+public:
+  /// Constructor
+  explicit JointLimitConstraint(dynamics::Joint* joint);
+
+  /// Destructor
+  ~JointLimitConstraint() override = default;
+
+  // 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();
+
+  //----------------------------------------------------------------------------
+  // 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:
+  /// The Joint that this constraint is associated with.
+  dynamics::Joint* mJoint;
+
+  /// The child BodyNode of the Joint that this constraint is associated with.
+  dynamics::BodyNode* mBodyNode;
+
+  /// Index of applied impulse
+  std::size_t mAppliedImpulseIndex;
+
+  /// Life time of constraint of each DOF.
+  Eigen::Matrix<std::size_t, 6, 1> mLifeTime;
+
+  /// Whether joint value exceeds the position limit.
+  Eigen::Matrix<bool, 6, 1> mIsPositionLimitViolated;
+
+  /// Whether joint value exceeds the velocity limit.
+  Eigen::Matrix<bool, 6, 1> mIsVelocityLimitViolated;
+
+  /// How much current joint values are exceeded from the limits.
+  Eigen::Matrix<double, 6, 1> mViolation;
+
+  /// The desired delta velocity to satisfy the joint limit constraint.
+  Eigen::Matrix<double, 6, 1> mNegativeVel;
+
+  /// Constraint impulse of the previous step.
+  Eigen::Matrix<double, 6, 1> mOldX;
+
+  /// Upper limit of the constraint impulse.
+  Eigen::Matrix<double, 6, 1> mUpperBound;
+
+  /// Lower limit of the constraint impulse.
+  Eigen::Matrix<double, 6, 1> mLowerBound;
+
+  /// 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_JOINTLIMITCONSTRAINT_HPP_

include/dart/constraint/LCPSolver.hpp

@@ -0,0 +1,71 @@
+/*
+ * 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_LCPSOLVER_HPP_
+#define DART_CONSTRAINT_LCPSOLVER_HPP_
+
+namespace dart {
+namespace constraint {
+
+class ConstrainedGroup;
+
+/// \deprecated This header has been deprecated in DART 6.7.
+///
+/// LCPSolver
+class LCPSolver
+{
+public:
+  /// Solve constriant impulses for a constrained group
+  virtual void solve(ConstrainedGroup* _group) = 0;
+
+  /// Set time step
+  void setTimeStep(double _timeStep);
+
+  /// Return time step
+  double getTimeStep() const;
+
+  /// Destructor
+  virtual ~LCPSolver();
+
+protected:
+  /// Constructor
+  LCPSolver(double _timeStep);
+
+protected:
+  /// Simulation time step
+  double mTimeStep;
+};
+
+} // namespace constraint
+} // namespace dart
+
+#endif // DART_CONSTRAINT_LCPSOLVER_HPP_

include/dart/constraint/MimicMotorConstraint.hpp

@@ -0,0 +1,164 @@
+/*
+ * 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_MIMICMOTORCONSTRAINT_HPP_
+#define DART_CONSTRAINT_MIMICMOTORCONSTRAINT_HPP_
+
+#include <dart/constraint/ConstraintBase.hpp>
+
+#include <dart/dynamics/MimicDofProperties.hpp>
+
+#include <vector>
+
+namespace dart {
+
+namespace dynamics {
+class BodyNode;
+class Joint;
+} // namespace dynamics
+
+namespace constraint {
+
+/// Servo motor constraint
+class MimicMotorConstraint : public ConstraintBase
+{
+public:
+  /// Constructor that creates a MimicMotorConstraint using the given
+  /// MimicDofProperties for each dependent joint's DoF.
+  /// \param[in] joint The dependent joint.
+  /// \param[in] mimicDofProperties A vector of MimicDofProperties for each DoF
+  /// of the dependent joint.
+  explicit MimicMotorConstraint(
+      dynamics::Joint* joint,
+      const std::vector<dynamics::MimicDofProperties>& mimicDofProperties);
+
+  /// Destructor
+  ~MimicMotorConstraint() override;
+
+  // 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:
+  /// Dependent joint whose motion is influenced by the reference joint.
+  dynamics::Joint* mJoint;
+
+  /// Vector of MimicDofProperties for the dependent joint.
+  std::vector<dynamics::MimicDofProperties> mMimicProps;
+
+  /// BodyNode associated with the dependent joint.
+  dynamics::BodyNode* mBodyNode;
+
+  /// Index of the applied impulse for the dependent joint.
+  std::size_t mAppliedImpulseIndex;
+
+  /// Array storing the lifetime of each constraint (in iterations).
+  std::size_t mLifeTime[6];
+  // TODO(JS): Lifetime should be considered only when we use iterative lcp
+  // solver
+
+  /// Array indicating whether each constraint is active or not.
+  bool mActive[6];
+
+  /// Array storing the negative velocity errors for each constraint.
+  double mNegativeVelocityError[6];
+
+  /// Array storing the previous values of the constraint forces.
+  double mOldX[6];
+
+  /// Array storing the upper bounds for the constraint forces.
+  double mUpperBound[6];
+
+  /// Array storing the lower bounds for the constraint forces.
+  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_MIMICMOTORCONSTRAINT_HPP_

include/dart/constraint/PGSLCPSolver.hpp

@@ -0,0 +1,109 @@
+/*
+ * 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_PGSLCPSOLVER_HPP_
+#define DART_CONSTRAINT_PGSLCPSOLVER_HPP_
+
+#include <dart/config.hpp>
+
+#include <dart/constraint/LCPSolver.hpp>
+
+#include <cstddef>
+
+namespace dart {
+namespace constraint {
+
+/// \deprecated This header has been deprecated in DART 6.7. Please include
+/// PgsBoxedLcpSolver.hpp instead.
+///
+/// PGSLCPSolver
+class PGSLCPSolver : public LCPSolver
+{
+public:
+  /// Constructor
+  explicit PGSLCPSolver(double _timestep);
+
+  /// Constructor
+  virtual ~PGSLCPSolver();
+
+  // Documentation inherited
+  void solve(ConstrainedGroup* _group) override;
+
+#if DART_BUILD_MODE_DEBUG
+private:
+  /// Return true if the matrix is symmetric
+  bool isSymmetric(std::size_t _n, double* _A);
+
+  /// Return true if the diagonal block of matrix is symmetric
+  bool isSymmetric(
+      std::size_t _n, double* _A, std::size_t _begin, std::size_t _end);
+
+  /// Print debug information
+  void print(
+      std::size_t _n,
+      double* _A,
+      double* _x,
+      double* _lo,
+      double* _hi,
+      double* _b,
+      double* w,
+      int* _findex);
+#endif
+};
+
+struct PGSOption
+{
+  int itermax;
+  double sor_w;
+  double eps_ea;
+  double eps_res;
+  double eps_div;
+
+  void setDefault();
+};
+
+bool solvePGS(
+    int n,
+    int nskip,
+    int /*nub*/,
+    double* A,
+    double* x,
+    double* b,
+    double* lo,
+    double* hi,
+    int* findex,
+    PGSOption* option);
+
+} // namespace constraint
+} // namespace dart
+
+#endif // DART_CONSTRAINT_PGSLCPSOLVER_HPP_