robotoc
robotoc - efficient ROBOT Optimal Control solvers
Loading...
Searching...
No Matches
constraints_impl.hpp
Go to the documentation of this file.
1#ifndef ROBOTOC_CONSTRAINTS_IMPL_HPP_
2#define ROBOTOC_CONSTRAINTS_IMPL_HPP_
3
4#include <vector>
5#include <memory>
6
7#include "robotoc/robot/robot.hpp"
8#include "robotoc/core/split_solution.hpp"
9#include "robotoc/core/split_kkt_residual.hpp"
10#include "robotoc/core/split_kkt_matrix.hpp"
11#include "robotoc/constraints/constraint_component_data.hpp"
12
13
14namespace robotoc {
15namespace constraintsimpl {
16
21template <typename ConstraintComponentBaseTypePtr>
22void clear(std::vector<ConstraintComponentBaseTypePtr>& constraints);
23
29template <typename ConstraintComponentBaseTypePtr>
30void createConstraintsData(
31 const std::vector<ConstraintComponentBaseTypePtr>& constraints,
32 std::vector<ConstraintComponentData>& data);
33
43template <typename ConstraintComponentBaseTypePtr, typename ContactStatusType>
44bool isFeasible(const std::vector<ConstraintComponentBaseTypePtr>& constraints,
45 Robot& robot, const ContactStatusType& contact_status,
46 std::vector<ConstraintComponentData>& data,
47 const SplitSolution& s);
48
57template <typename ConstraintComponentBaseTypePtr, typename ContactStatusType>
58void setSlackAndDual(
59 const std::vector<ConstraintComponentBaseTypePtr>& constraints,
60 Robot& robot, const ContactStatusType& contact_status,
61 std::vector<ConstraintComponentData>& data, const SplitSolution& s);
62
72template <typename ConstraintComponentBaseTypePtr, typename ContactStatusType>
73void evalConstraint(
74 const std::vector<ConstraintComponentBaseTypePtr>& constraints,
75 Robot& robot, const ContactStatusType& contact_status,
76 std::vector<ConstraintComponentData>& data, const SplitSolution& s);
77
88template <typename ConstraintComponentBaseTypePtr, typename ContactStatusType>
89void linearizeConstraints(
90 const std::vector<ConstraintComponentBaseTypePtr>& constraints,
91 Robot& robot, const ContactStatusType& contact_status,
92 std::vector<ConstraintComponentData>& data, const SplitSolution& s,
93 SplitKKTResidual& kkt_residual);
94
106template <typename ConstraintComponentBaseTypePtr, typename ContactStatusType>
107void condenseSlackAndDual(
108 const std::vector<ConstraintComponentBaseTypePtr>& constraints,
109 const ContactStatusType& contact_status,
110 std::vector<ConstraintComponentData>& data, SplitKKTMatrix& kkt_matrix,
111 SplitKKTResidual& kkt_residual);
112
121template <typename ConstraintComponentBaseTypePtr, typename ContactStatusType,
122 typename SplitDirectionType>
123void expandSlackAndDual(
124 const std::vector<ConstraintComponentBaseTypePtr>& constraints,
125 const ContactStatusType& contact_status,
126 std::vector<ConstraintComponentData>& data, const SplitDirectionType& d);
127
134template <typename ConstraintComponentBaseTypePtr>
135double maxSlackStepSize(
136 const std::vector<ConstraintComponentBaseTypePtr>& constraints,
137 const std::vector<ConstraintComponentData>& data);
138
145template <typename ConstraintComponentBaseTypePtr>
146double maxDualStepSize(
147 const std::vector<ConstraintComponentBaseTypePtr>& constraints,
148 const std::vector<ConstraintComponentData>& data);
149
155void updateSlack(std::vector<ConstraintComponentData>& data,
156 const double step_size);
157
163void updateDual(std::vector<ConstraintComponentData>& data,
164 const double step_size);
165
171template <typename ConstraintComponentBaseTypePtr>
172void setBarrierParam(std::vector<ConstraintComponentBaseTypePtr>& constraints,
173 const double barrier_param);
174
181template <typename ConstraintComponentBaseTypePtr>
182void setFractionToBoundaryRule(
183 std::vector<ConstraintComponentBaseTypePtr>& constraints,
184 const double fraction_to_boundary_rule);
185
186} // namespace constraintsimpl
187} // namespace robotoc
188
189#include "robotoc/constraints/constraints_impl.hxx"
190
191#endif // ROBOTOC_CONSTRAINTS_IMPL_HPP_
robotoc::Robot
Dynamics and kinematics model of robots. Wraps pinocchio::Model and pinocchio::Data....
Definition: robot.hpp:32
robotoc::SplitKKTMatrix
The KKT matrix split into a time stage.
Definition: split_kkt_matrix.hpp:18
robotoc::SplitKKTResidual
KKT residual split into each time stage.
Definition: split_kkt_residual.hpp:18
robotoc::SplitSolution
Solution to the optimal control problem split into a time stage.
Definition: split_solution.hpp:20
constraint_component_data.hpp
constraints_impl.hxx
robotoc::constraintsimpl::expandSlackAndDual
void expandSlackAndDual(const std::vector< ConstraintComponentBaseTypePtr > &constraints, const ContactStatusType &contact_status, std::vector< ConstraintComponentData > &data, const SplitDirectionType &d)
Expands the slack and dual, i.e., computes the directions of the slack and dual variables from the di...
Definition: constraints_impl.hxx:118
robotoc::constraintsimpl::setBarrierParam
void setBarrierParam(std::vector< ConstraintComponentBaseTypePtr > &constraints, const double barrier_param)
Sets the barrier parameter.
Definition: constraints_impl.hxx:186
robotoc::constraintsimpl::condenseSlackAndDual
void condenseSlackAndDual(const std::vector< ConstraintComponentBaseTypePtr > &constraints, const ContactStatusType &contact_status, std::vector< ConstraintComponentData > &data, SplitKKTMatrix &kkt_matrix, SplitKKTResidual &kkt_residual)
Condenses the slack and dual variables. linearizeConstraints() must be called before this function.
Definition: constraints_impl.hxx:101
robotoc::constraintsimpl::maxSlackStepSize
double maxSlackStepSize(const std::vector< ConstraintComponentBaseTypePtr > &constraints, const std::vector< ConstraintComponentData > &data)
Computes and returns the maximum step size by applying fraction-to-boundary-rule to the direction of ...
Definition: constraints_impl.hxx:132
robotoc::constraintsimpl::setFractionToBoundaryRule
void setFractionToBoundaryRule(std::vector< ConstraintComponentBaseTypePtr > &constraints, const double fraction_to_boundary_rule)
Sets the parameter of the fraction-to-boundary-rule.
Definition: constraints_impl.hxx:195
robotoc::constraintsimpl::createConstraintsData
void createConstraintsData(const std::vector< ConstraintComponentBaseTypePtr > &constraints, std::vector< ConstraintComponentData > &data)
Creates constraints data.
Definition: constraints_impl.hxx:19
robotoc::constraintsimpl::evalConstraint
void evalConstraint(const std::vector< ConstraintComponentBaseTypePtr > &constraints, Robot &robot, const ContactStatusType &contact_status, std::vector< ConstraintComponentData > &data, const SplitSolution &s)
Computes the primal residual, residual in the complementary slackness, and the log-barrier_param func...
Definition: constraints_impl.hxx:66
robotoc::constraintsimpl::updateSlack
void updateSlack(std::vector< ConstraintComponentData > &data, const double step_size)
Updates the slack variables according to the step size.
Definition: constraints_impl.hxx:167
robotoc::constraintsimpl::maxDualStepSize
double maxDualStepSize(const std::vector< ConstraintComponentBaseTypePtr > &constraints, const std::vector< ConstraintComponentData > &data)
Computes and returns the maximum step size by applying fraction-to-boundary-rule to the direction of ...
Definition: constraints_impl.hxx:150
robotoc::constraintsimpl::isFeasible
bool isFeasible(const std::vector< ConstraintComponentBaseTypePtr > &constraints, Robot &robot, const ContactStatusType &contact_status, std::vector< ConstraintComponentData > &data, const SplitSolution &s)
Checks whether the current solution s is feasible or not.
Definition: constraints_impl.hxx:33
robotoc::constraintsimpl::clear
void clear(std::vector< ConstraintComponentBaseTypePtr > &constraints)
Clears the vector of the constraints.
Definition: constraints_impl.hxx:13
robotoc::constraintsimpl::linearizeConstraints
void linearizeConstraints(const std::vector< ConstraintComponentBaseTypePtr > &constraints, Robot &robot, const ContactStatusType &contact_status, std::vector< ConstraintComponentData > &data, const SplitSolution &s, SplitKKTResidual &kkt_residual)
Evaluates the constraints (i.e., calls evalConstraint()) and adds the products of the Jacobian of the...
Definition: constraints_impl.hxx:82
robotoc::constraintsimpl::setSlackAndDual
void setSlackAndDual(const std::vector< ConstraintComponentBaseTypePtr > &constraints, Robot &robot, const ContactStatusType &contact_status, std::vector< ConstraintComponentData > &data, const SplitSolution &s)
Sets the slack and dual variables of each constraint components.
Definition: constraints_impl.hxx:51
robotoc::constraintsimpl::updateDual
void updateDual(std::vector< ConstraintComponentData > &data, const double step_size)
Updates the dual variables according to the step size.
Definition: constraints_impl.hxx:176
robotoc
Definition: constraint_component_base.hpp:17
split_kkt_matrix.hpp
split_kkt_residual.hpp
split_solution.hpp