dartpy 7.0.0.dev0__cp313-cp313-manylinux_2_39_x86_64.whl

include/dart/optimizer/Function.hpp

@@ -0,0 +1,203 @@
+/*
+ * 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_OPTIMIZER_FUNCTION_HPP_
+#define DART_OPTIMIZER_FUNCTION_HPP_
+
+#include <Eigen/Dense>
+
+#include <functional>
+#include <memory>
+#include <vector>
+
+namespace dart {
+namespace optimizer {
+
+class Function
+{
+public:
+  /// Constructor
+  explicit Function(const std::string& _name = "function");
+
+  /// Destructor
+  virtual ~Function();
+
+  /// Sets the name of this Function
+  virtual void setName(const std::string& newName);
+
+  /// Returns the name of this Function
+  const std::string& getName() const;
+
+  /// Evaluates and returns the objective function at the point x
+  virtual double eval(const Eigen::VectorXd& x) = 0;
+
+  /// Evaluates and returns the objective function at the point x
+  virtual void evalGradient(
+      const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad);
+
+  /// Evaluates and return the objective function at the point x.
+  ///
+  /// If you have a raw array that the gradient will be passed in, then use
+  /// evalGradient(const Eigen::VectorXd&, Eigen::Map<Eigen::VectorXd>)
+  /// for better performance.
+  void evalGradient(const Eigen::VectorXd& _x, Eigen::VectorXd& _grad);
+
+  /// Evaluates and return the objective function at the point x
+  virtual void evalHessian(
+      const Eigen::VectorXd& _x,
+      Eigen::Map<Eigen::VectorXd, Eigen::RowMajor> _Hess);
+
+protected:
+  /// Name of this function
+  std::string mName;
+};
+
+typedef std::shared_ptr<Function> FunctionPtr;
+typedef std::unique_ptr<Function> UniqueFunctionPtr;
+
+typedef std::function<double(const Eigen::VectorXd&)> CostFunction;
+
+typedef std::function<void(const Eigen::VectorXd&, Eigen::Map<Eigen::VectorXd>)>
+    GradientFunction;
+
+typedef std::function<void(
+    const Eigen::VectorXd&, Eigen::Map<Eigen::VectorXd, Eigen::RowMajor>)>
+    HessianFunction;
+
+/// ModularFunction uses C++11 std::function to allow you to easily swap
+/// out the cost function, gradient function, and Hessian function during
+/// runtime for an optimizer::Function instance.
+class ModularFunction : public Function
+{
+public:
+  /// Constructor
+  explicit ModularFunction(const std::string& _name = "modular_function");
+
+  /// Destructor
+  ~ModularFunction() override;
+
+  /// eval() will now call whatever CostFunction you set using
+  /// setCostFunction()
+  double eval(const Eigen::VectorXd& _x) override;
+
+  /// evalGradient() will now call whatever GradientFunction you set
+  /// using setGradientFunction()
+  void evalGradient(
+      const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad) override;
+
+  /// evalHessian() will now call whatever HessianFunction you set using
+  /// setHessianFunction()
+  void evalHessian(
+      const Eigen::VectorXd& _x,
+      Eigen::Map<Eigen::VectorXd, Eigen::RowMajor> _Hess) override;
+
+  /// Set the function that gets called by eval()
+  void setCostFunction(CostFunction _cost);
+
+  /// Replace the cost function with a constant-zero function. Passing in
+  /// true will cause a warning to be printed out whenever eval() is called.
+  void clearCostFunction(bool _printWarning = true);
+
+  /// Set the function that gets called by evalGradient()
+  void setGradientFunction(GradientFunction _gradient);
+
+  /// Replace the gradient function with the default evalGradient() of
+  /// the base Function class. A warning will be printed whenever evalGradient()
+  /// gets called.
+  void clearGradientFunction();
+
+  /// Set the function that gets called by evalHessian()
+  void setHessianFunction(HessianFunction _hessian);
+
+  /// Replace the Hessian function with the default evalHessian() of the
+  /// base Function class. A warning will be printed whenever evalHessian() gets
+  /// called.
+  void clearHessianFunction();
+
+protected:
+  /// Storage for the cost function
+  CostFunction mCostFunction;
+
+  /// Storage for the gradient function
+  GradientFunction mGradientFunction;
+
+  /// Storage for the Hessian function
+  HessianFunction mHessianFunction;
+};
+
+/// NullFunction is a constant-zero Function
+class NullFunction : public Function
+{
+public:
+  /// Constructor
+  explicit NullFunction(const std::string& _name = "null_function");
+
+  /// Destructor
+  ~NullFunction() override;
+
+  /// eval() will always return exactly zero
+  double eval(const Eigen::VectorXd&) override;
+
+  /// evalGradient will always set _grad to a zero vector that
+  /// matches the dimensionality of _x
+  void evalGradient(
+      const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad) override;
+
+  /// evalHessian() will always set _Hess to a zero matrix that matches
+  /// the dimensionality of _x
+  void evalHessian(
+      const Eigen::VectorXd& _x,
+      Eigen::Map<Eigen::VectorXd, Eigen::RowMajor> _Hess) override;
+};
+
+/// class MultiFunction
+class MultiFunction
+{
+public:
+  /// Constructor
+  MultiFunction();
+
+  /// Destructor
+  virtual ~MultiFunction();
+
+  /// Operator ()
+  virtual void operator()(
+      const Eigen::VectorXd& _x,
+      Eigen::Map<Eigen::VectorXd>& _f,
+      Eigen::Map<Eigen::MatrixXd>& _grad)
+      = 0;
+};
+
+} // namespace optimizer
+} // namespace dart
+
+#endif // DART_OPTIMIZER_FUNCTION_HPP_

include/dart/optimizer/GenericMultiObjectiveProblem.hpp

@@ -0,0 +1,174 @@
+/*
+ * 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_OPTIMIZER_GENERICMULTIOBJECTIVEPROBLEM_HPP_
+#define DART_OPTIMIZER_GENERICMULTIOBJECTIVEPROBLEM_HPP_
+
+#include <dart/optimizer/Function.hpp>
+#include <dart/optimizer/MultiObjectiveProblem.hpp>
+
+#include <Eigen/Dense>
+
+#include <vector>
+
+#include <cstddef>
+
+namespace dart {
+namespace optimizer {
+
+class GenericMultiObjectiveProblem : public MultiObjectiveProblem
+{
+public:
+  /// Constructor
+  explicit GenericMultiObjectiveProblem(
+      std::size_t dim, std::size_t integerDim = 0u);
+
+  /// Destructor
+  ~GenericMultiObjectiveProblem() override = default;
+
+  /// \{ \name Objectives
+
+  // Documentation inherited.
+  std::size_t getObjectiveDimension() const override;
+
+  /// Sets objective functions to be minimized.
+  void setObjectiveFunctions(const std::vector<FunctionPtr>& objectives);
+
+  /// Adds a minimum objective function
+  void addObjectiveFunction(FunctionPtr objective);
+
+  /// Returns the number objective functions.
+  std::size_t getNumObjectiveFunctions() const;
+
+  /// Returns objective functions
+  const std::vector<FunctionPtr>& getObjectiveFunctions() const;
+
+  /// Removes an objective function
+  void removeObjectiveFunction(FunctionPtr function);
+
+  /// Removes all objective functions
+  void removeAllObjectiveFunctions();
+
+  /// \}
+
+  /// \{ \name Equality Constraints
+
+  // Documentation inherited.
+  std::size_t getEqConstraintDimension() const override;
+
+  /// Adds equality constraint
+  void addEqConstraintFunction(FunctionPtr eqConst);
+
+  /// Returns number of equality constraints
+  std::size_t getNumEqualityConstraintFunctions() const;
+
+  /// Returns equality constraint
+  FunctionPtr getEqConstraintFunction(std::size_t index) const;
+
+  /// Removes equality constraint
+  void removeEqConstraintFunction(FunctionPtr eqConst);
+
+  /// Removes all equality constraints
+  void removeAllEqConstraintFunctions();
+
+  /// \}
+
+  /// \{ \name Inequality Constraints
+
+  // Documentation inherited.
+  std::size_t getIneqConstraintDimension() const override;
+
+  /// Adds inequality constraint. Inequality constraints must evaluate
+  /// to LESS THAN or equal to zero (within some tolerance) to be satisfied.
+  void addIneqConstraintFunction(FunctionPtr ineqConst);
+
+  /// Returns number of inequality constraints
+  std::size_t getNumIneqConstraintFunctions() const;
+
+  /// Returns inequality constraint
+  FunctionPtr getIneqConstraintFunction(std::size_t index) const;
+
+  /// Removes inequality constraint
+  void removeIneqConstraintFunction(FunctionPtr ineqConst);
+
+  /// Removes all inequality constraints
+  void removeAllIneqConstraintFunctions();
+
+  /// \}
+
+  /// \{ \name Evaluations
+
+  /// Evaluates objectives
+  Eigen::VectorXd evaluateObjectives(const Eigen::VectorXd& x) const override;
+
+  /// Evaluates equality constraints
+  Eigen::VectorXd evaluateEqConstraints(
+      const Eigen::VectorXd& x) const override;
+
+  /// Evaluates inequality constraints
+  Eigen::VectorXd evaluateIneqConstraints(
+      const Eigen::VectorXd& x) const override;
+
+  /// Return dimension of fitness
+  std::size_t getFitnessDimension() const;
+
+  /// Evaluates fitness, which is [objectives, equality constraints, inequality
+  /// constraints].
+  Eigen::VectorXd evaluateFitness(const Eigen::VectorXd& x) const;
+
+  /// \}
+
+protected:
+  /// Objective functions
+  std::vector<FunctionPtr> mObjectiveFunctions;
+
+  /// Equality constraint functions
+  std::vector<FunctionPtr> mEqConstraintFunctions;
+
+  /// Inequality constraint functions
+  std::vector<FunctionPtr> mIneqConstraintFunctions;
+
+private:
+  /// Cache for objective dimension
+  std::size_t mObjectiveDimension;
+
+  /// Cache for equality constraint dimension
+  std::size_t mEqConstraintDimension;
+
+  /// Cache for inequality constraint dimension
+  std::size_t mIneqConstraintDimension;
+};
+
+} // namespace optimizer
+} // namespace dart
+
+#endif // DART_OPTIMIZER_GENERICMULTIOBJECTIVEPROBLEM_HPP_

include/dart/optimizer/GradientDescentSolver.hpp

@@ -0,0 +1,240 @@
+/*
+ * 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_OPTIMIZER_GRADIENTDESCENTSOLVER_HPP_
+#define DART_OPTIMIZER_GRADIENTDESCENTSOLVER_HPP_
+
+#include <dart/optimizer/Solver.hpp>
+
+#include <random>
+
+namespace dart {
+namespace optimizer {
+
+/// DefaultSolver is a Solver extension which is native to DART (rather than
+/// relying on third-party libraries). It uses randomized gradient descent and
+/// softened constraints (i.e. constraint functions are added into the
+/// objective function and assigned weights) to solve nonlinear problems. Note
+/// that this is not a good option for Problems with difficult constraint
+/// functions that need to be solved exactly.
+class GradientDescentSolver : public Solver
+{
+public:
+  static const std::string Type;
+
+  struct UniqueProperties
+  {
+    /// Value of the fixed step size
+    double mStepSize;
+
+    /// Number of attempts to make before quitting. Each attempt will start from
+    /// the next seed provided by the problem. Once there are no more seeds,
+    /// random starting configurations will be used.
+    ///
+    /// Set this to 0 to keep trying until a solution is found (the program will
+    /// need to be interrupted in order to stop if no solution is being found).
+    std::size_t mMaxAttempts;
+
+    /// The number of steps between random perturbations being applied to the
+    /// configuration. Set this to 0 to never apply randomized perturbations.
+    std::size_t mPerturbationStep;
+
+    /// The random perturbation works as follows: A random point in the domain
+    /// of the Problem is selected, and then a random step size between 0 and
+    /// mMaxPerturbationFactor is selected. The configuration will take a step
+    /// of that random step size towards the random point.
+    ///
+    /// A maximum value of 1.0 is recommended for mMaxPerturbationFactor. A
+    /// smaller value will result in smaller randomized perturbations. A value
+    /// significantly larger than 1.0 could bias the configuration towards the
+    /// boundary of the Problem domain.
+    double mMaxPerturbationFactor;
+
+    /// The largest permittable change in value when randomizing a configuration
+    double mMaxRandomizationStep;
+
+    /// This is the weight that will be applied to any constraints that do not
+    /// have a corresponding weight specified by mEqConstraintWeights or by
+    /// mIneqConstraintWeights
+    double mDefaultConstraintWeight;
+
+    /// Vector of weights that should be applied to the equality constraints.
+    /// If there are fewer components in this vector than there are equality
+    /// constraints in the Problem, then the remaining equality constraints will
+    /// be assigned a weight of mDefaultConstraintWeight.
+    Eigen::VectorXd mEqConstraintWeights;
+
+    /// Vector of weights that should be applied to the inequality constraints.
+    /// If there are fewer components in this vector than there are inequality
+    /// constraints in the Problem, then the remaining inequality constraints
+    /// will be assigned a weight of mDefaultConstraintWeight.
+    Eigen::VectorXd mIneqConstraintWeights;
+
+    UniqueProperties(
+        double _stepMultiplier = 0.1,
+        std::size_t _maxAttempts = 1,
+        std::size_t _perturbationStep = 0,
+        double _maxPerturbationFactor = 1.0,
+        double _maxRandomizationStep = 1e10,
+        double _defaultConstraintWeight = 1.0,
+        Eigen::VectorXd _eqConstraintWeights = Eigen::VectorXd(),
+        Eigen::VectorXd _ineqConstraintWeights = Eigen::VectorXd());
+  };
+
+  struct Properties : Solver::Properties, UniqueProperties
+  {
+    Properties(
+        const Solver::Properties& _solverProperties = Solver::Properties(),
+        const UniqueProperties& _descentProperties = UniqueProperties());
+  };
+
+  /// Default constructor
+  explicit GradientDescentSolver(const Properties& _properties = Properties());
+
+  /// Alternative constructor
+  explicit GradientDescentSolver(std::shared_ptr<Problem> _problem);
+
+  /// Destructor
+  virtual ~GradientDescentSolver();
+
+  // Documentation inherited
+  bool solve() override;
+
+  /// Get the last configuration that was used by the Solver
+  Eigen::VectorXd getLastConfiguration() const;
+
+  // Documentation inherited
+  std::string getType() const override;
+
+  // Documentation inherited
+  std::shared_ptr<Solver> clone() const override;
+
+  /// Set the Properties of this GradientDescentSolver
+  void setProperties(const Properties& _properties);
+
+  /// Set the Properties of this GradientDescentSolver
+  void setProperties(const UniqueProperties& _properties);
+
+  /// Get the Properties of this GradientDescentSolver
+  Properties getGradientDescentProperties() const;
+
+  /// Copy the Properties of another GradientDescentSolver
+  void copy(const GradientDescentSolver& _other);
+
+  /// Copy the Properties of another GradientDescentSolver
+  GradientDescentSolver& operator=(const GradientDescentSolver& _other);
+
+  /// Set the multiplier for the step size
+  void setStepSize(double _newMultiplier);
+
+  /// Get the multiplier for the step size
+  double getStepSize() const;
+
+  /// Set the maximum number of solving attempts before quitting. Each attempt
+  /// will use getNumMaxIterations() steps. When a new attempt is started, it
+  /// will use the next seed in the list of seeds. If we've reached the end of
+  /// the list of seeds, the attempt will start from a randomized configuration.
+  void setMaxAttempts(std::size_t _maxAttempts);
+
+  /// Get the maximum number of solving attempts.
+  std::size_t getMaxAttempts() const;
+
+  /// Set the number of steps that will be taken before applying a randomized
+  /// perturbation.
+  void setPerturbationStep(std::size_t _step);
+
+  /// Get UniqueProperties::mPerturbationStep
+  std::size_t getPerturbationStep() const;
+
+  /// Set UniqueProperties::mPerturbationFactor
+  void setMaxPerturbationFactor(double _factor);
+
+  /// Get UniqueProperties::mPerturbationFactor
+  double getMaxPerturbationFactor() const;
+
+  /// Set UniqueProperties::mDefaultConstraintWeight
+  void setDefaultConstraintWeight(double _newDefault);
+
+  /// Get UniqueProperties::mDefaultConstraintWeight
+  double getDefaultConstraintWeight() const;
+
+  /// Set UniqueProperties::mEqConstraintWeights
+  Eigen::VectorXd& getEqConstraintWeights();
+
+  /// Get UniqueProperties::mEqConstraintWeights
+  const Eigen::VectorXd& getEqConstraintWeights() const;
+
+  /// Set UniqueProperties::mIneqConstraintWeights
+  Eigen::VectorXd& getIneqConstraintWeights();
+
+  /// Get UniqueProperties::mIneqConstraintWeights
+  const Eigen::VectorXd& getIneqConstraintWeights() const;
+
+  /// Randomize the configuration based on this Solver's settings
+  void randomizeConfiguration(Eigen::VectorXd& _x);
+
+  /// Clamp the configuration to the limits of the Problem
+  void clampToBoundary(Eigen::VectorXd& _x);
+
+  /// Get the number of iterations used in the last attempt to solve the problem
+  std::size_t getLastNumIterations() const;
+
+protected:
+  /// GradientDescentSolver properties
+  UniqueProperties mGradientP;
+
+  /// The last number of iterations performed by this Solver
+  std::size_t mLastNumIterations;
+
+  /// Randomization device
+  std::random_device mRD;
+
+  /// Mersenne twister method
+  std::mt19937 mMT;
+
+  /// Distribution
+  std::uniform_real_distribution<double> mDistribution;
+
+  /// Cache to track the costs of equality constraints
+  Eigen::VectorXd mEqConstraintCostCache;
+
+  /// Cache to track the costs of inequality constraints
+  Eigen::VectorXd mIneqConstraintCostCache;
+
+  /// The last config reached by this Solver
+  Eigen::VectorXd mLastConfig;
+};
+
+} // namespace optimizer
+} // namespace dart
+
+#endif // DART_OPTIMIZER_GRADIENTDESCENTSOLVER_HPP_