diff --git a/ifopt_core/include/ifopt/bounds.h b/ifopt_core/include/ifopt/bounds.h index 41b1752..9f60b5c 100644 --- a/ifopt_core/include/ifopt/bounds.h +++ b/ifopt_core/include/ifopt/bounds.h @@ -33,12 +33,12 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ifopt { /** - * @brief Upper and lower bound for optimization variables and constraints. - */ + * @brief Upper and lower bound for optimization variables and constraints. + */ struct Bounds { /** - * @brief Creates a bound between @a lower and @a upper. - */ + * @brief Creates a bound between @a lower and @a upper. + */ Bounds(double lower = 0.0, double upper = 0.0) { lower_ = lower; diff --git a/ifopt_core/include/ifopt/composite.h b/ifopt_core/include/ifopt/composite.h index 7f7a5a5..88412d0 100644 --- a/ifopt_core/include/ifopt/composite.h +++ b/ifopt_core/include/ifopt/composite.h @@ -48,18 +48,18 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ifopt { /** - * @brief Interface representing either Variable, Cost or Constraint. - * - * Variables, costs and constraints can all be fit into the same interface - * (Component). For example, each has a "value", which is either the actual - * value of the variables, the constraint value g or the cost. - * This representation provides one common interface - * ("smallest common denominator") that can be contain either individual - * variables/costs/constraints or a Composite of these. This pattern - * takes care of stacking variables, ordering Jacobians and providing bounds - * for the complete problem without duplicating code. For more information on - * the composite pattern visit https://sourcemaking.com/design_patterns/composite - */ + * @brief Interface representing either Variable, Cost or Constraint. + * + * Variables, costs and constraints can all be fit into the same interface + * (Component). For example, each has a "value", which is either the actual + * value of the variables, the constraint value g or the cost. + * This representation provides one common interface + * ("smallest common denominator") that can be contain either individual + * variables/costs/constraints or a Composite of these. This pattern + * takes care of stacking variables, ordering Jacobians and providing bounds + * for the complete problem without duplicating code. For more information on + * the composite pattern visit https://sourcemaking.com/design_patterns/composite + */ class Component { public: using Ptr = std::shared_ptr; @@ -69,76 +69,78 @@ class Component { using VecBound = std::vector; /** - * @brief Creates a component. - * @param num_rows The number of rows of this components. - * @param name The identifier for this component. - * - * The number of rows @c num_rows can represent either - * @li number of variables in this variables set - * @li number of constraints in this constraint set - * @li 1 if this component represents a Cost. - */ + * @brief Creates a component. + * @param num_rows The number of rows of this components. + * @param name The identifier for this component. + * + * The number of rows @c num_rows can represent either + * @li number of variables in this variables set + * @li number of constraints in this constraint set + * @li 1 if this component represents a Cost. + */ Component(int num_rows, const std::string& name); virtual ~Component() = default; /** - * @brief Returns the "values" of whatever this component represents. - * - * @li For Variable this represents the actual optimization values. - * @li For Constraint this represents the constraint value g. - * @li For Cost this represents the cost value. - */ + * @brief Returns the "values" of whatever this component represents. + * + * @li For Variable this represents the actual optimization values. + * @li For Constraint this represents the constraint value g. + * @li For Cost this represents the cost value. + */ virtual VectorXd GetValues() const = 0; /** - * @brief Returns the "bounds" of this component. - * - * @li For Variable these are the upper and lower variable bound. - * @li For Constraint this represents the constraint bounds. - * @li For Cost these done't exists (set to infinity). - */ + * @brief Returns the "bounds" of this component. + * + * @li For Variable these are the upper and lower variable bound. + * @li For Constraint this represents the constraint bounds. + * @li For Cost these done't exists (set to infinity). + */ virtual VecBound GetBounds() const = 0; /** - * @brief Sets the optimization variables from an Eigen vector. - * - * This is only done for Variable, where these are set from the current - * values of the @ref solvers. - */ + * @brief Sets the optimization variables from an Eigen vector. + * + * This is only done for Variable, where these are set from the current + * values of the @ref solvers. + */ virtual void SetVariables(const VectorXd& x) = 0; /** - * @brief Returns derivatives of each row w.r.t. the variables - * - * @li For Constraint this is a matrix with one row per constraint. - * @li For a Cost this is a row vector (gradient transpose). - * @li Not sensible for Variable. - */ - virtual Jacobian GetJacobian() const = 0; - + * @brief Returns derivatives of each row w.r.t. the variables + * + * @li For Constraint this is a matrix with one row per constraint. + * @li For a Cost this is a row vector (gradient transpose). + * @li Not sensible for Variable. + */ + virtual Jacobian GetJacobian() const = 0; + virtual Jacobian GetHession(double obj_factor, + const double* lambuda) const = 0; + virtual Jacobian GetSingleHession(int irow) const = 0; /** - * @brief Returns the number of rows of this component. - */ + * @brief Returns the number of rows of this component. + */ int GetRows() const; /** - * @brief Returns the name (id) of this component. - */ + * @brief Returns the name (id) of this component. + */ std::string GetName() const; /** - * @brief Prints the relevant information (name, rows, values) of this component. - * @param tolerance When to flag constraint/bound violation. - * @param index_start Of this specific variables-, constraint- or cost set. - */ + * @brief Prints the relevant information (name, rows, values) of this component. + * @param tolerance When to flag constraint/bound violation. + * @param index_start Of this specific variables-, constraint- or cost set. + */ virtual void Print(double tolerance, int& index_start) const; /** - * @brief Sets the number of rows of this component. - * - * @attention This should correctly be done through constructor call, only - * delay this by using @c kSpecifyLater if you have good reason. - */ + * @brief Sets the number of rows of this component. + * + * @attention This should correctly be done through constructor call, only + * delay this by using @c kSpecifyLater if you have good reason. + */ void SetRows(int num_rows); static const int kSpecifyLater = -1; @@ -148,75 +150,82 @@ class Component { }; /** - * @brief A collection of components which is treated as another Component. - * - * This class follows the Component interface as well, but doesn't actually - * do any evaluation, but only stitches together the results of the - * components it is holding. This is where multiple sets of variables, - * constraints or costs are ordered and combined. - * - * See Component and Composite Pattern for more information. - */ + * @brief A collection of components which is treated as another Component. + * + * This class follows the Component interface as well, but doesn't actually + * do any evaluation, but only stitches together the results of the + * components it is holding. This is where multiple sets of variables, + * constraints or costs are ordered and combined. + * + * See Component and Composite Pattern for more information. + */ class Composite : public Component { public: using Ptr = std::shared_ptr; using ComponentVec = std::vector; /** - * @brief Creates a Composite holding either variables, costs or constraints. - * @param is_cost True if this class holds cost terms, false for all others. - * - * Constraints and variables append individual constraint sets and Jacobian - * rows below one another, whereas costs terms are all accumulated to a - * scalar value/a single Jacobian row. - */ + * @brief Creates a Composite holding either variables, costs or constraints. + * @param is_cost True if this class holds cost terms, false for all others. + * + * Constraints and variables append individual constraint sets and Jacobian + * rows below one another, whereas costs terms are all accumulated to a + * scalar value/a single Jacobian row. + */ Composite(const std::string& name, bool is_cost); virtual ~Composite() = default; // see Component for documentation VectorXd GetValues() const override; Jacobian GetJacobian() const override; + + Jacobian GetHession(double obj_factor, const double* lambuda) const override; + Jacobian GetSingleHession(int irow) const override; + VecBound GetBounds() const override; void SetVariables(const VectorXd& x) override; void PrintAll() const; /** - * @brief Access generic component with the specified name. - * @param name The name given to the component. - * @return A generic pointer of that component. - */ + * @brief Access generic component with the specified name. + * @param name The name given to the component. + * @return A generic pointer of that component. + */ const Component::Ptr GetComponent(std::string name) const; /** - * @brief Access type-casted component with the specified name. - * @param name The name given to the component. - * @tparam T Type of component. - * @return A type-casted pointer possibly providing addtional functionality. - */ + * @brief Access type-casted component with the specified name. + * @param name The name given to the component. + * @tparam T Type of component. + * @return A type-casted pointer possibly providing addtional functionality. + */ template std::shared_ptr GetComponent(const std::string& name) const; /** - * @brief Adds a component to this composite. - */ + * @brief Adds a component to this composite. + */ void AddComponent(const Component::Ptr&); /** - * @brief Removes all component from this composite. - */ + * @brief Removes all component from this composite. + */ void ClearComponents(); /** - * @brief Returns read access to the components. - */ + * @brief Returns read access to the components. + */ const ComponentVec GetComponents() const; + size_t GetMvar() const; + private: ComponentVec components_; bool is_cost_; // The number of variables for costs/constraint composites (not set for variables). // Is initialized the first the GetJacobian() is called. mutable size_t n_var = -1; + mutable size_t m_var = -1; }; // implementation of template functions diff --git a/ifopt_core/include/ifopt/constraint_set.h b/ifopt_core/include/ifopt/constraint_set.h index 6b04aad..ffacc2b 100644 --- a/ifopt_core/include/ifopt/constraint_set.h +++ b/ifopt_core/include/ifopt/constraint_set.h @@ -35,97 +35,120 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ifopt { /** - * @brief A container holding a set of related constraints. - * - * This container holds constraints representing a single concept, e.g. - * @c n constraints keeping a foot inside its range of motion. Each of the - * @c n rows is given by: - * lower_bound < g(x) < upper_bound - * - * These constraint sets are later then stitched together to form the overall - * problem. - * - * @ingroup ProblemFormulation - * @sa Component - */ + * @brief A container holding a set of related constraints. + * + * This container holds constraints representing a single concept, e.g. + * @c n constraints keeping a foot inside its range of motion. Each of the + * @c n rows is given by: + * lower_bound < g(x) < upper_bound + * + * These constraint sets are later then stitched together to form the overall + * problem. + * + * @ingroup ProblemFormulation + * @sa Component + */ class ConstraintSet : public Component { public: using Ptr = std::shared_ptr; using VariablesPtr = Composite::Ptr; /** - * @brief Creates constraints on the variables @c x. - * @param n_constraints The number of constraints. - * @param name What these constraints represent. - */ + * @brief Creates constraints on the variables @c x. + * @param n_constraints The number of constraints. + * @param name What these constraints represent. + */ ConstraintSet(int n_constraints, const std::string& name); virtual ~ConstraintSet() = default; /** - * @brief Connects the constraint with the optimization variables. - * @param x A pointer to the current values of the optimization variables. - * - * The optimization variable values are necessary for calculating constraint - * violations and Jacobians. - */ + * @brief Connects the constraint with the optimization variables. + * @param x A pointer to the current values of the optimization variables. + * + * The optimization variable values are necessary for calculating constraint + * violations and Jacobians. + */ void LinkWithVariables(const VariablesPtr& x); - + Jacobian GetHession(double obj_factor, const double* lambuda) const + { + throw std::runtime_error("GetHession not implemented for ConstraintSet"); + } /** - * @brief The matrix of derivatives for these constraints and variables. - * - * Assuming @c n constraints and @c m variables, the returned Jacobian - * has dimensions n x m. Every row represents the derivatives of a single - * constraint, whereas every column refers to a single optimization variable. - * - * This function only combines the user-defined jacobians from - * FillJacobianBlock(). - */ + * @brief The matrix of derivatives for these constraints and variables. + * + * Assuming @c n constraints and @c m variables, the returned Jacobian + * has dimensions n x m. Every row represents the derivatives of a single + * constraint, whereas every column refers to a single optimization variable. + * + * This function only combines the user-defined jacobians from + * FillJacobianBlock(). + */ Jacobian GetJacobian() const final; - /** - * @brief Set individual Jacobians corresponding to each decision variable set. - * @param var_set Set of variables the current Jacobian block belongs to. - * @param jac_block Columns of the overall Jacobian affected by var_set. - * - * A convenience function so the user does not have to worry about the - * ordering of variable sets. All that is required is that the user knows - * the internal ordering of variables in each individual set and provides - * the Jacobian of the constraints w.r.t. this set (starting at column 0). - * GetJacobian() then inserts these columns at the correct position in the - * overall Jacobian. - * - * If the constraint doen't depend on a @c var_set, this function should - * simply do nothing. - * - * Attention: @c jac_bock is a sparse matrix, and must always have the same - * sparsity pattern. Therefore, it's better not to build a dense matrix and - * call .sparseView(), because if some entries happen to be zero for some - * iteration, that changes the sparsity pattern. A more robust way is to directly - * set as follows (which can also be set =0.0 without erros): - * jac_block.coeffRef(1, 3) = ... - */ + * @brief The matrix of Hessian for these constraints and variables. + * @param irow the ith row of constraints,for cost,i equals to zero. + + * Assuming @c n constraints and @c m variables, The returned Hessian matrix is + * the Hessian matrix of the constraint or objective function of the row,which + * has dimensions m x m. + + */ + Jacobian GetSingleHession(int irow) const; + /** + * @brief Set individual Jacobians corresponding to each decision variable set. + * @param var_set Set of variables the current Jacobian block belongs to. + * @param jac_block Columns of the overall Jacobian affected by var_set. + * + * A convenience function so the user does not have to worry about the + * ordering of variable sets. All that is required is that the user knows + * the internal ordering of variables in each individual set and provides + * the Jacobian of the constraints w.r.t. this set (starting at column 0). + * GetJacobian() then inserts these columns at the correct position in the + * overall Jacobian. + * + * If the constraint doen't depend on a @c var_set, this function should + * simply do nothing. + * + * Attention: @c jac_bock is a sparse matrix, and must always have the same + * sparsity pattern. Therefore, it's better not to build a dense matrix and + * call .sparseView(), because if some entries happen to be zero for some + * iteration, that changes the sparsity pattern. A more robust way is to directly + * set as follows (which can also be set =0.0 without erros): + * jac_block.coeffRef(1, 3) = ... + */ virtual void FillJacobianBlock(std::string var_set, Jacobian& jac_block) const = 0; + /** + * @brief Set individual Hessian corresponding to each decision variable set. + * @param irow the ith row of constraints,for cost,i equals to zero. + * @param var_set Set of variables the current Hessian block belongs to. + * @param jac_block Hessian to return,for convinence using jac_block to represent. + * Assuming @c n constraints and @c m variables, The returned Hessian matrix is + * the Hessian matrix of the constraint or objective function of the row,which + * has dimensions m x m. + */ + virtual void FillHessionBlock(std::string var_set, Jacobian& jac_block, + int irow) const = 0; protected: /** - * @brief Read access to the value of the optimization variables. - * - * This must be used to formulate the constraint violation and Jacobian. - */ + * @brief Read access to the value of the optimization variables. + * + * This must be used to formulate the constraint violation and Jacobian. + */ const VariablesPtr GetVariables() const { return variables_; }; private: VariablesPtr variables_; /** - * @brief Initialize quantities that depend on the optimization variables. - * @param x A pointer to the initial values of the optimization variables. - * - * Sometimes the number of constraints depends on the variable representation, - * or shorthands to specific variable sets want to be saved for quicker - * access later. This function can be overwritten for that. - */ + * @brief Initialize quantities that depend on the optimization variables. + * @param x A pointer to the initial values of the optimization variables. + * + * Sometimes the number of constraints depends on the variable representation, + * or shorthands to specific variable sets want to be saved for quicker + * access later. This function can be overwritten for that. + */ virtual void InitVariableDependedQuantities(const VariablesPtr& x_init){}; // doesn't exist for constraints, generated run-time error when used diff --git a/ifopt_core/include/ifopt/cost_term.h b/ifopt_core/include/ifopt/cost_term.h index 18727bd..1ffb80c 100644 --- a/ifopt_core/include/ifopt/cost_term.h +++ b/ifopt_core/include/ifopt/cost_term.h @@ -35,14 +35,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ifopt { /** - * @brief A container holding a single cost term. - * - * This container builds a scalar cost term from the values of the variables. - * This can be seen as a constraint with only one row and no bounds. - * - * @ingroup ProblemFormulation - * @sa Component - */ + * @brief A container holding a single cost term. + * + * This container builds a scalar cost term from the values of the variables. + * This can be seen as a constraint with only one row and no bounds. + * + * @ingroup ProblemFormulation + * @sa Component + */ class CostTerm : public ConstraintSet { public: CostTerm(const std::string& name); @@ -50,24 +50,32 @@ class CostTerm : public ConstraintSet { private: /** - * @brief Returns the scalar cost term calculated from the @c variables. - */ + * @brief Returns the scalar cost term calculated from the @c variables. + */ virtual double GetCost() const = 0; public: /** - * @brief Wrapper function that converts double to Eigen::VectorXd. - */ + * @brief Wrapper function that converts double to Eigen::VectorXd. + */ VectorXd GetValues() const final; /** - * @brief Returns infinite bounds (e.g. no bounds). - */ + * @brief in case this function get called in costterm + */ + Jacobian GetHession(double obj_factor, double* lambuda) const + { + throw std::runtime_error("not implemented for CostTerm"); + }; + + /** + * @brief Returns infinite bounds (e.g. no bounds). + */ VecBound GetBounds() const final; /** - * Cost term printout slightly different from variables/constraints. - */ + * Cost term printout slightly different from variables/constraints. + */ void Print(double tol, int& index) const final; }; diff --git a/ifopt_core/include/ifopt/problem.h b/ifopt_core/include/ifopt/problem.h index 4a3321f..7dd71ca 100644 --- a/ifopt_core/include/ifopt/problem.h +++ b/ifopt_core/include/ifopt/problem.h @@ -40,59 +40,59 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ifopt { /** - * @defgroup ProblemFormulation - * @brief The elements to formulate the solver independent optimization problem. - * - * An optimization problem usually consists of multiple sets of independent - * variable- or constraint-sets. Each set represents a common concept, e.g. one - * set of variables represents spline coefficients, another footstep positions. - * Similarly, each constraint-set groups a set of similar constraints. - * - * The Nonlinear Optimization Problem to solve is defined as: - * - * find x0, x1 (variable-sets 0 & 1) - * s.t - * x0_lower <= x0 <= x0_upper (bounds on variable-set x0 \in R^2) - * - * {x0,x1} = arg min c0(x0,x1)+c1(x0,x1) (cost-terms 0 and 1) - * - * g0_lower < g0(x0,x1) < g0_upper (constraint-set 0 \in R^2) - * g1_lower < g1(x0,x1) < g0_upper (constraint-set 1 \in R^1) - * - * - * #### GetValues() - * This structure allows a user to define each of these sets independently in - * separate classes and ifopt takes care of building the overall problem from - * these sets. This is implemented by - * * stacking all variable-sets to build the overall variable vector - * * summing all cost-terms to calculate the total cost - * * stacking all constraint-sets to build the overall constraint vector. - * - * #### GetJacobian() - * Supplying derivative information greatly increases solution speed. ifopt - * allows to define the derivative of each cost-term/constraint-set with - * respect to each variable-set independently. This ensures that when the - * order of variable-sets changes in the overall vector, this derivative - * information is still valid. These "Jacobian blocks" must be supplied through - * ConstraintSet::FillJacobianBlock() and are then used to build the complete Jacobian for - * the cost and constraints. - * - * \image html ifopt.png - */ + * @defgroup ProblemFormulation + * @brief The elements to formulate the solver independent optimization problem. + * + * An optimization problem usually consists of multiple sets of independent + * variable- or constraint-sets. Each set represents a common concept, e.g. one + * set of variables represents spline coefficients, another footstep positions. + * Similarly, each constraint-set groups a set of similar constraints. + * + * The Nonlinear Optimization Problem to solve is defined as: + * + * find x0, x1 (variable-sets 0 & 1) + * s.t + * x0_lower <= x0 <= x0_upper (bounds on variable-set x0 \in R^2) + * + * {x0,x1} = arg min c0(x0,x1)+c1(x0,x1) (cost-terms 0 and 1) + * + * g0_lower < g0(x0,x1) < g0_upper (constraint-set 0 \in R^2) + * g1_lower < g1(x0,x1) < g0_upper (constraint-set 1 \in R^1) + * + * + * #### GetValues() + * This structure allows a user to define each of these sets independently in + * separate classes and ifopt takes care of building the overall problem from + * these sets. This is implemented by + * * stacking all variable-sets to build the overall variable vector + * * summing all cost-terms to calculate the total cost + * * stacking all constraint-sets to build the overall constraint vector. + * + * #### GetJacobian() + * Supplying derivative information greatly increases solution speed. ifopt + * allows to define the derivative of each cost-term/constraint-set with + * respect to each variable-set independently. This ensures that when the + * order of variable-sets changes in the overall vector, this derivative + * information is still valid. These "Jacobian blocks" must be supplied through + * ConstraintSet::FillJacobianBlock() and are then used to build the complete Jacobian for + * the cost and constraints. + * + * \image html ifopt.png + */ /** - * @brief A generic optimization problem with variables, costs and constraints. - * - * This class is responsible for holding all the information of an optimization - * problem, which includes the optimization variables, their variable bounds, - * the cost function, the constraints and their bounds and derivatives of - * all. With this information the problem can be solved by any specific solver. - * All the quantities (variables, cost, constraint) are represented - * by the same generic Component class. - * - * @ingroup ProblemFormulation - * See @ref Solvers for currently implemented solvers. - */ + * @brief A generic optimization problem with variables, costs and constraints. + * + * This class is responsible for holding all the information of an optimization + * problem, which includes the optimization variables, their variable bounds, + * the cost function, the constraints and their bounds and derivatives of + * all. With this information the problem can be solved by any specific solver. + * All the quantities (variables, cost, constraint) are represented + * by the same generic Component class. + * + * @ingroup ProblemFormulation + * See @ref Solvers for currently implemented solvers. + */ class Problem { public: using VecBound = Component::VecBound; @@ -100,169 +100,183 @@ class Problem { using VectorXd = Component::VectorXd; /** - * @brief Creates a optimization problem with no variables, costs or constraints. - */ + * @brief Creates a optimization problem with no variables, costs or constraints. + */ Problem(); virtual ~Problem() = default; /** - * @brief Add one individual set of variables to the optimization problem. - * @param variable_set The selection of optimization variables. - * - * This function can be called multiple times, with multiple sets, e.g. - * one set that parameterizes a body trajectory, the other that resembles - * the optimal timing values. This function correctly appends the - * individual variables sets and ensures correct order of Jacobian columns. - */ + * @brief Add one individual set of variables to the optimization problem. + * @param variable_set The selection of optimization variables. + * + * This function can be called multiple times, with multiple sets, e.g. + * one set that parameterizes a body trajectory, the other that resembles + * the optimal timing values. This function correctly appends the + * individual variables sets and ensures correct order of Jacobian columns. + */ void AddVariableSet(VariableSet::Ptr variable_set); /** - * @brief Add a set of multiple constraints to the optimization problem. - * @param constraint_set This can be 1 to infinity number of constraints. - * - * This function can be called multiple times for different sets of - * constraints. It makes sure the overall constraint and Jacobian correctly - * considers all individual constraint sets. - */ + * @brief Add a set of multiple constraints to the optimization problem. + * @param constraint_set This can be 1 to infinity number of constraints. + * + * This function can be called multiple times for different sets of + * constraints. It makes sure the overall constraint and Jacobian correctly + * considers all individual constraint sets. + */ void AddConstraintSet(ConstraintSet::Ptr constraint_set); /** - * @brief Add a cost term to the optimization problem. - * @param cost_set The calculation of the cost from the variables. - * - * This function can be called multiple times if the cost function is - * composed of different cost terms. It makes sure the overall value and - * gradient is considering each individual cost. - */ + * @brief Add a cost term to the optimization problem. + * @param cost_set The calculation of the cost from the variables. + * + * This function can be called multiple times if the cost function is + * composed of different cost terms. It makes sure the overall value and + * gradient is considering each individual cost. + */ void AddCostSet(CostTerm::Ptr cost_set); /** - * @brief Updates the variables with the values of the raw pointer @c x. - */ + * @brief Updates the variables with the values of the raw pointer @c x. + */ void SetVariables(const double* x); /** - * @brief The number of optimization variables. - */ + * @brief The number of optimization variables. + */ int GetNumberOfOptimizationVariables() const; /** - * @brief True if the optimization problem includes a cost, false if - * merely a feasibility problem is defined. - */ + * @brief True if the optimization problem includes a cost, false if + * merely a feasibility problem is defined. + */ bool HasCostTerms() const; /** - * @brief The maximum and minimum value each optimization variable - * is allowed to have. - */ + * @brief The maximum and minimum value each optimization variable + * is allowed to have. + */ VecBound GetBoundsOnOptimizationVariables() const; /** - * @brief The current value of the optimization variables. - */ + * @brief The current value of the optimization variables. + */ VectorXd GetVariableValues() const; /** - * @brief The scalar cost for current optimization variables @c x. - */ + * @brief The scalar cost for current optimization variables @c x. + */ double EvaluateCostFunction(const double* x); /** - * @brief The column-vector of derivatives of the cost w.r.t. each variable. - * @details ipopt uses 10e-8 for their derivative check, but setting here to more precise - * https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_derivative_test_perturbation - */ + * @brief The column-vector of derivatives of the cost w.r.t. each variable. + * @details ipopt uses 10e-8 for their derivative check, but setting here to more precise + * https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_derivative_test_perturbation + */ VectorXd EvaluateCostFunctionGradient( const double* x, bool use_finite_difference_approximation = false, double epsilon = std::numeric_limits::epsilon()); /** - * @brief The number of individual constraints. - */ + * @brief The number of individual constraints. + */ int GetNumberOfConstraints() const; /** - * @brief The upper and lower bound of each individual constraint. - */ + * @brief The upper and lower bound of each individual constraint. + */ VecBound GetBoundsOnConstraints() const; /** - * @brief Each constraint value g(x) for current optimization variables @c x. - */ + * @brief Each constraint value g(x) for current optimization variables @c x. + */ VectorXd EvaluateConstraints(const double* x); /** - * @brief Extracts those entries from constraint Jacobian that are not zero. - * @param [in] x The current values of the optimization variables. - * @param [out] values The nonzero derivatives ordered by Eigen::RowMajor. - */ + * @brief Extracts those entries from constraint Jacobian that are not zero. + * @param [in] x The current values of the optimization variables. + * @param [out] values The nonzero derivatives ordered by Eigen::RowMajor. + */ void EvalNonzerosOfJacobian(const double* x, double* values); - /** - * @brief The sparse-matrix representation of Jacobian of the constraints. - * - * Each row corresponds to a constraint and each column to an optimizaton - * variable. - */ + * @brief Extracts those entries from Hessian that are not zero. + * @param [in] x The current values of the optimization variables. + * @param [out] values The nonzero 2nd order derivatives ordered by Eigen::RowMajor. + */ + void EvalNonzerosOfHession(const double* x, double* values, double obj_factor, + const double* lambda); + /** + * @brief The sparse-matrix representation of Jacobian of the constraints. + * + * Each row corresponds to a constraint and each column to an optimizaton + * variable. + */ Jacobian GetJacobianOfConstraints() const; /** - * @brief The sparse-matrix representation of Jacobian of the costs. - * - * Returns one row corresponding to the costs and each column corresponding - * to an optimizaton variable. - */ + * @brief The sparse-matrix representation of Hessian . + * + * Calculate the weighted sum of the Hessian matrix of the + * objective function and the constraint function + */ + + Jacobian GetHessionOfCosts(double obj_factor, const double* lambda); + /** + * @brief The sparse-matrix representation of Jacobian of the costs. + * + * Returns one row corresponding to the costs and each column corresponding + * to an optimizaton variable. + */ Jacobian GetJacobianOfCosts() const; /** - * @brief Saves the current values of the optimization variables in x_prev. - * - * This is used to keep a history of the values for each NLP iterations. - */ + * @brief Saves the current values of the optimization variables in x_prev. + * + * This is used to keep a history of the values for each NLP iterations. + */ void SaveCurrent(); /** - * @brief Read/write access to the current optimization variables. - */ + * @brief Read/write access to the current optimization variables. + */ Composite::Ptr GetOptVariables() const; /** - * @brief Sets the optimization variables to those at iteration iter. - */ + * @brief Sets the optimization variables to those at iteration iter. + */ void SetOptVariables(int iter); /** - * @brief Sets the optimization variables to those of the final iteration. - */ + * @brief Sets the optimization variables to those of the final iteration. + */ void SetOptVariablesFinal(); /** - * @brief The number of iterations it took to solve the problem. - */ + * @brief The number of iterations it took to solve the problem. + */ int GetIterationCount() const { return x_prev.size(); }; /** - * @brief Prints the variables, costs and constraints. - */ + * @brief Prints the variables, costs and constraints. + */ void PrintCurrent() const; /** - * @brief Read access to the constraints composite - * @return A const reference to constraints_ - */ + * @brief Read access to the constraints composite + * @return A const reference to constraints_ + */ const Composite& GetConstraints() const { return constraints_; }; /** - * @brief Read access to the costs composite - * @return A const reference to costs_ - */ + * @brief Read access to the costs composite + * @return A const reference to costs_ + */ const Composite& GetCosts() const { return costs_; }; /** - * @brief Read access to the history of iterations - * @return A const reference to x_prev - */ + * @brief Read access to the history of iterations + * @return A const reference to x_prev + */ const std::vector& GetIterations() const { return x_prev; }; private: diff --git a/ifopt_core/include/ifopt/variable_set.h b/ifopt_core/include/ifopt/variable_set.h index 9fc9ece..fbf74e4 100644 --- a/ifopt_core/include/ifopt/variable_set.h +++ b/ifopt_core/include/ifopt/variable_set.h @@ -35,21 +35,21 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ifopt { /** - * @brief A container holding a set of related optimization variables. - * - * This is a single set of variables representing a single concept, e.g - * "spline coefficients" or "step durations". - * - * @ingroup ProblemFormulation - * @sa Component - */ + * @brief A container holding a set of related optimization variables. + * + * This is a single set of variables representing a single concept, e.g + * "spline coefficients" or "step durations". + * + * @ingroup ProblemFormulation + * @sa Component + */ class VariableSet : public Component { public: /** - * @brief Creates a set of variables representing a single concept. - * @param n_var Number of variables. - * @param name What the variables represent to (e.g. "spline coefficients"). - */ + * @brief Creates a set of variables representing a single concept. + * @param n_var Number of variables. + * @param name What the variables represent to (e.g. "spline coefficients"). + */ VariableSet(int n_var, const std::string& name); virtual ~VariableSet() = default; @@ -58,6 +58,20 @@ class VariableSet : public Component { { throw std::runtime_error("not implemented for variables"); }; + /** + * @brief in case this function get called in VariableSet + */ + Jacobian GetHession(double obj_factor, const double* lambuda) const + { + throw std::runtime_error("not implemented for variables"); + }; + /** + * @brief in case this function get called in VariableSet + */ + Jacobian GetSingleHession(int irow) const + { + throw std::runtime_error("not implemented for variables"); + }; }; } // namespace ifopt diff --git a/ifopt_core/src/composite.cc b/ifopt_core/src/composite.cc index 831aeba..d448767 100644 --- a/ifopt_core/src/composite.cc +++ b/ifopt_core/src/composite.cc @@ -175,6 +175,76 @@ Composite::Jacobian Composite::GetJacobian() const return jacobian; } +Component::Jacobian Composite::GetHession(double obj_factor, + const double* lambuda) const +{ + if (m_var == -1) { + m_var = components_.empty() + ? 0 + : components_.front()->GetSingleHession(0).cols(); + } + Jacobian Hes(m_var, m_var); + Jacobian Hes_Transition_vessel(m_var * 2, m_var * 2); + //Prevents triples from exceeding the maximum range of the matrix + int maximum_element = m_var * m_var; + int elementcounter = 0; + if (m_var == 0) { + return Hes; + } + int row = 0; + std::vector> triplet_list; + double intermidiate; + for (const auto& c : components_) { + int SIZE = c->GetRows(); + for (int i = 0; i < SIZE; i++) { + if (!is_cost_) { + intermidiate = lambuda[i + row]; + } else { + intermidiate = obj_factor; + } + const Jacobian& jac = intermidiate * c->GetSingleHession(i); + triplet_list.reserve(triplet_list.size() + jac.nonZeros()); + for (int k = 0; k < jac.outerSize(); ++k) { + for (Jacobian::InnerIterator it(jac, k); it; ++it) { + triplet_list.push_back( + Eigen::Triplet(it.row(), it.col(), it.value())); + elementcounter += 1; + if (elementcounter == maximum_element) { + Hes_Transition_vessel.setFromTriplets(triplet_list.begin(), + triplet_list.end()); + triplet_list.clear(); + elementcounter = 0; + //Prevents triples from exceeding the maximum range of the matrix + for (int kin = 0; kin < Hes_Transition_vessel.outerSize(); ++kin) { + for (Jacobian::InnerIterator itin(Hes_Transition_vessel, kin); + itin; ++itin) { + triplet_list.push_back(Eigen::Triplet( + itin.row(), itin.col(), itin.value())); + elementcounter += 1; + } + } + Hes_Transition_vessel.setZero(); + //If the triplet has more elements than the final matrix, + //use an intermediate matrix to transition + } + } + } + } + + if (!is_cost_) { + row += c->GetRows(); + } + } + Hes.setFromTriplets(triplet_list.begin(), triplet_list.end()); + return Hes; +} + +Component::Jacobian Composite::GetSingleHession(int irow) const +{ + throw std::runtime_error("GetSingleHession not implemented for Composite"); + //this will not be valid +} + Composite::VecBound Composite::GetBounds() const { VecBound bounds_; @@ -191,6 +261,11 @@ const Composite::ComponentVec Composite::GetComponents() const return components_; } +size_t Composite::GetMvar() const +{ + return m_var; +} + void Composite::PrintAll() const { int index = 0; diff --git a/ifopt_core/src/leaves.cc b/ifopt_core/src/leaves.cc index 6e03800..f0a1e5f 100644 --- a/ifopt_core/src/leaves.cc +++ b/ifopt_core/src/leaves.cc @@ -73,6 +73,52 @@ ConstraintSet::Jacobian ConstraintSet::GetJacobian() const return jacobian; } +Component::Jacobian ConstraintSet::GetSingleHession(int irow) const +{ + Jacobian Hes(variables_->GetRows(), variables_->GetRows()); + Jacobian Hes_Transition_vessel(10 + variables_->GetRows(), + 10 + variables_->GetRows()); + int maximum_element = variables_->GetRows() * variables_->GetRows(); + int col = 0; + Jacobian Heslocal; + std::vector> triplet_list; + int elementcounter = 0; + for (const auto& vars : variables_->GetComponents()) { + int n = vars->GetRows(); + Heslocal.resize(n, n); + + FillHessionBlock(vars->GetName(), Heslocal, irow); + triplet_list.reserve(triplet_list.size() + Heslocal.nonZeros()); + + for (int k = 0; k < Heslocal.outerSize(); ++k) { + for (Jacobian::InnerIterator it(Heslocal, k); it; ++it) { + triplet_list.push_back( + Eigen::Triplet(it.row(), col + it.col(), it.value())); + elementcounter += 1; + if (elementcounter == maximum_element) { + Hes_Transition_vessel.setFromTriplets(triplet_list.begin(), + triplet_list.end()); + triplet_list.clear(); + elementcounter = 0; + for (int kin = 0; kin < Hes_Transition_vessel.outerSize(); ++kin) { + for (Jacobian::InnerIterator itin(Hes_Transition_vessel, kin); itin; + ++itin) { + triplet_list.push_back( + Eigen::Triplet(itin.row(), itin.col(), itin.value())); + elementcounter += 1; + } + } + Hes_Transition_vessel.setZero(); + } + } + } + col += n; + } + + Hes.setFromTriplets(triplet_list.begin(), triplet_list.end()); + return Hes; +} + void ConstraintSet::LinkWithVariables(const VariablesPtr& x) { variables_ = x; diff --git a/ifopt_core/src/problem.cc b/ifopt_core/src/problem.cc index d472563..af53666 100644 --- a/ifopt_core/src/problem.cc +++ b/ifopt_core/src/problem.cc @@ -143,11 +143,53 @@ void Problem::EvalNonzerosOfJacobian(const double* x, double* values) std::copy(jac.valuePtr(), jac.valuePtr() + jac.nonZeros(), values); } +void Problem::EvalNonzerosOfHession(const double* x, double* values, + double obj_factor, const double* lambda) +{ + SetVariables(x); + Jacobian Hes = GetHessionOfCosts(obj_factor, lambda); + + Hes.makeCompressed(); // so the valuePtr() is dense and accurate + std::copy(Hes.valuePtr(), Hes.valuePtr() + Hes.nonZeros(), values); +} + Problem::Jacobian Problem::GetJacobianOfConstraints() const { return constraints_.GetJacobian(); } +Problem::Jacobian Problem::GetHessionOfCosts(double obj_factor, + const double* lambda) +{ + //Compute the Hessian matrix of the objective function and the constraint respectively + Jacobian part_1 = constraints_.GetHession(obj_factor, lambda); + Jacobian part_2 = costs_.GetHession(obj_factor, lambda); + int num1 = constraints_.GetMvar(); + int num2 = costs_.GetMvar(); + size_t Hessize = (num1 > num2) ? num1 : num2; + Jacobian Hes(Hessize, Hessize); + + std::vector> triplet_list; + triplet_list.reserve(triplet_list.size() + part_1.nonZeros()); + for (int k = 0; k < part_1.outerSize(); ++k) { + for (Jacobian::InnerIterator it(part_1, k); it; ++it) { + triplet_list.push_back( + Eigen::Triplet(it.row(), it.col(), it.value())); + } + } + // + triplet_list.reserve(triplet_list.size() + part_2.nonZeros()); + for (int k = 0; k < part_2.outerSize(); ++k) { + for (Jacobian::InnerIterator it(part_2, k); it; ++it) { + triplet_list.push_back( + Eigen::Triplet(it.row(), it.col(), it.value())); + } + } + + Hes.setFromTriplets(triplet_list.begin(), triplet_list.end()); + return Hes; +} + Problem::Jacobian Problem::GetJacobianOfCosts() const { return costs_.GetJacobian();