robotoc
robotoc - efficient ROBOT Optimal Control solvers
|
Base class for constraint components. More...
#include <constraint_component_base.hpp>
Public Member Functions | |
ConstraintComponentBase (const double barrier_param=1.0e-03, const double fraction_to_boundary_rule=0.995) | |
Constructor. More... | |
virtual | ~ConstraintComponentBase () |
Destructor. More... | |
ConstraintComponentBase (const ConstraintComponentBase &)=default | |
Default copy constructor. More... | |
ConstraintComponentBase & | operator= (const ConstraintComponentBase &)=default |
Default copy operator. More... | |
ConstraintComponentBase (ConstraintComponentBase &&) noexcept=default | |
Default move constructor. More... | |
ConstraintComponentBase & | operator= (ConstraintComponentBase &&) noexcept=default |
Default move assign operator. More... | |
virtual KinematicsLevel | kinematicsLevel () const =0 |
Checks the kinematics level of the constraint component. More... | |
virtual void | allocateExtraData (ConstraintComponentData &data) const =0 |
Allocates extra data in ConstraintComponentData. More... | |
virtual bool | isFeasible (Robot &robot, const ContactStatus &contact_status, ConstraintComponentData &data, const SplitSolution &s) const =0 |
Checks whether the current solution s is feasible or not. More... | |
virtual void | setSlack (Robot &robot, const ContactStatus &contact_status, ConstraintComponentData &data, const SplitSolution &s) const =0 |
Sets the slack variables of each constraint components. More... | |
virtual void | evalConstraint (Robot &robot, const ContactStatus &contact_status, ConstraintComponentData &data, const SplitSolution &s) const =0 |
Computes the primal residual, residual in the complementary slackness, and the log-barrier function of the slack varible. More... | |
virtual void | evalDerivatives (Robot &robot, const ContactStatus &contact_status, ConstraintComponentData &data, const SplitSolution &s, SplitKKTResidual &kkt_residual) const =0 |
Computes the derivatives of the priaml residual, i.e., the Jacobian of the inequality constraint, and add the product of the Jacobian and the dual variable to the KKT residual. This function is always called just after evalConstraint(). More... | |
virtual void | condenseSlackAndDual (const ContactStatus &contact_status, ConstraintComponentData &data, SplitKKTMatrix &kkt_matrix, SplitKKTResidual &kkt_residual) const =0 |
Condenses the slack and dual variables, i.e., factorizes the condensed Hessians and KKT residuals. This function is always called just after evalDerivatives(). More... | |
virtual void | expandSlackAndDual (const ContactStatus &contact_status, ConstraintComponentData &data, const SplitDirection &d) const =0 |
Expands the slack and dual, i.e., computes the directions of the slack and dual variables from the directions of the primal variables. More... | |
virtual int | dimc () const =0 |
Returns the size of the constraint. More... | |
void | setSlackAndDualPositive (ConstraintComponentData &data) const |
Sets the slack and dual variables positive. More... | |
double | maxSlackStepSize (const ConstraintComponentData &data) const |
Computes and returns the maximum step size by applying fraction-to-boundary-rule to the direction of the slack variable. More... | |
double | maxDualStepSize (const ConstraintComponentData &data) const |
Computes and returns the maximum step size by applying fraction-to-boundary-rule to the direction of the dual variable. More... | |
virtual double | getBarrierParam () const final |
Gets the barrier parameter. More... | |
virtual double | getFractionToBoundaryRule () const final |
Gets the margin parameter of the fraction-to-boundary-rule. More... | |
virtual void | setBarrierParam (const double barrier_param) final |
Sets the barrier parameter. More... | |
virtual void | setFractionToBoundaryRule (const double fraction_to_boundary_rule) final |
Sets the margin parameter of the fraction-to-boundary-rule. More... | |
template<typename Derived > | |
std::shared_ptr< Derived > | as_shared_ptr () |
Gets the shared ptr of this object as the specified type. If this fails in dynamic casting, throws an exception. More... | |
Static Public Member Functions | |
static void | updateSlack (ConstraintComponentData &data, const double step_size) |
Updates the slack variable according to the step size. More... | |
static void | updateDual (ConstraintComponentData &data, const double step_size) |
Updates the dual variable according to the step size. More... | |
Protected Member Functions | |
void | computeComplementarySlackness (ConstraintComponentData &data) const |
Computes the residual in the complementarity slackness between the slack and dual variables. More... | |
void | computeComplementarySlackness (ConstraintComponentData &data, const int start, const int size) const |
Computes the residual in the complementarity slackness between the slack and dual variables. More... | |
template<int Size> | |
void | computeComplementarySlackness (ConstraintComponentData &data, const int start) const |
Computes the residual in the complementarity slackness between the slack and dual variables. More... | |
double | computeComplementarySlackness (const double slack, const double dual) const |
Computes the residual in the complementarity slackness between the slack and dual variables. More... | |
template<typename VectorType > | |
double | logBarrier (const Eigen::MatrixBase< VectorType > &slack) const |
Computes the log barrier function of the slack variable. More... | |
Static Protected Member Functions | |
static void | computeCondensingCoeffcient (ConstraintComponentData &data) |
Computes the coefficient of the condensing. More... | |
static void | computeCondensingCoeffcient (ConstraintComponentData &data, const int start, const int size) |
Computes the coefficient of the condensing. More... | |
template<int Size> | |
static void | computeCondensingCoeffcient (ConstraintComponentData &data, const int start) |
Computes the coefficient of the condensing. More... | |
static double | computeCondensingCoeffcient (const double slack, const double dual, const double residual, const double cmpl) |
Computes the residual in the complementarity slackness between the slack and dual variables. More... | |
static void | computeDualDirection (ConstraintComponentData &data) |
Computes the direction of the dual variable from slack, primal residual, complementary slackness, and the direction of the slack. More... | |
static void | computeDualDirection (ConstraintComponentData &data, const int start, const int size) |
Computes the direction of the dual variable from slack, primal residual, complementary slackness, and the direction of the slack. More... | |
template<int Size> | |
static void | computeDualDirection (ConstraintComponentData &data, const int start) |
Computes the direction of the dual variable from slack, primal residual, complementary slackness, and the direction of the slack. More... | |
static double | computeDualDirection (const double slack, const double dual, const double dslack, const double cmpl) |
Computes the direction of the dual variable from slack, primal residual, complementary slackness, and the direction of the slack. More... | |
Base class for constraint components.
robotoc::ConstraintComponentBase::ConstraintComponentBase | ( | const double | barrier_param = 1.0e-03 , |
const double | fraction_to_boundary_rule = 0.995 |
||
) |
Constructor.
[in] | barrier_param | Barrier parameter. Must be positive. Should be small. Default is 1.0e-03. |
[in] | fraction_to_boundary_rule | Parameter of the fraction-to-boundary-rule Must be larger than 0 and smaller than 1. Should be between 0.9 and 0.995. Default is 0.995. |
|
inlinevirtual |
Destructor.
|
default |
Default copy constructor.
|
defaultnoexcept |
Default move constructor.
|
pure virtual |
Allocates extra data in ConstraintComponentData.
[in] | data | Constraint component data. |
Implemented in robotoc::ContactWrenchCone, robotoc::FrictionCone, robotoc::JointAccelerationLowerLimit, robotoc::JointAccelerationUpperLimit, robotoc::JointPositionLowerLimit, robotoc::JointPositionUpperLimit, robotoc::JointTorquesLowerLimit, robotoc::JointTorquesUpperLimit, robotoc::JointVelocityLowerLimit, and robotoc::JointVelocityUpperLimit.
|
inline |
Gets the shared ptr of this object as the specified type. If this fails in dynamic casting, throws an exception.
Derived | The derived type. |
|
inlineprotected |
Computes the residual in the complementarity slackness between
the slack and dual variables.
[in] | slack | An element of the slack variable. |
[in] | dual | An element of the dual variable. |
|
inlineprotected |
Computes the residual in the complementarity slackness between
the slack and dual variables.
[in,out] | data | Constraint data. |
|
inlineprotected |
Computes the residual in the complementarity slackness between
the slack and dual variables.
[in,out] | data | Constraint data. |
[in] | start | Start position of the segment. |
Size | Size of the segment. |
|
inlineprotected |
Computes the residual in the complementarity slackness between
the slack and dual variables.
[in,out] | data | Constraint data. |
[in] | start | Start position of the segment. |
[in] | size | Size of the segment. |
|
inlinestaticprotected |
Computes the residual in the complementarity slackness between
the slack and dual variables.
[in] | slack | An element of the slack variable. |
[in] | dual | An element of the dual variable. |
[in] | residual | An element of the primal residual. |
[in] | cmpl | An element of the complementarity slackness. |
|
inlinestaticprotected |
Computes the coefficient of the condensing.
[in,out] | data | Constraint component data. |
|
inlinestaticprotected |
Computes the coefficient of the condensing.
[in,out] | data | Constraint data. |
[in] | start | Start position of the segment. |
Size | Size of the segment. |
|
inlinestaticprotected |
Computes the coefficient of the condensing.
[in,out] | data | Constraint data. |
[in] | start | Start position of the segment. |
[in] | size | Size of the segment. |
|
inlinestaticprotected |
Computes the direction of the dual variable from slack, primal residual, complementary slackness, and the direction of the slack.
[in] | slack | The slack variable. |
[in] | dual | The dual variable. |
[in] | dslack | The direction of the slack variable. |
[in] | cmpl | The residual in the complementary slackness. |
|
inlinestaticprotected |
Computes the direction of the dual variable from slack, primal residual, complementary slackness, and the direction of the slack.
[in,out] | data | Constraint data. |
|
inlinestaticprotected |
Computes the direction of the dual variable from slack, primal residual, complementary slackness, and the direction of the slack.
[in,out] | data | Constraint data. |
[in] | start | Start position of the segment. |
Size | Size of the segment. |
|
inlinestaticprotected |
Computes the direction of the dual variable from slack, primal residual, complementary slackness, and the direction of the slack.
[in,out] | data | Constraint data. |
[in] | start | Start position of the segment. |
[in] | size | Size of the segment. |
|
pure virtual |
Condenses the slack and dual variables, i.e., factorizes the
condensed Hessians and KKT residuals. This function is always called just after evalDerivatives().
[in] | contact_status | Contact status. |
[in] | data | Constraint data. |
[out] | kkt_matrix | Split KKT matrix. The condensed Hessians are added to this object. |
[out] | kkt_residual | Split KKT residual. The condensed residuals are added to this object. |
Implemented in robotoc::ContactWrenchCone, robotoc::FrictionCone, robotoc::JointAccelerationLowerLimit, robotoc::JointAccelerationUpperLimit, robotoc::JointPositionLowerLimit, robotoc::JointPositionUpperLimit, robotoc::JointTorquesLowerLimit, robotoc::JointTorquesUpperLimit, robotoc::JointVelocityLowerLimit, and robotoc::JointVelocityUpperLimit.
|
pure virtual |
Returns the size of the constraint.
Implemented in robotoc::ContactWrenchCone, robotoc::FrictionCone, robotoc::JointAccelerationLowerLimit, robotoc::JointAccelerationUpperLimit, robotoc::JointPositionLowerLimit, robotoc::JointPositionUpperLimit, robotoc::JointTorquesLowerLimit, robotoc::JointTorquesUpperLimit, robotoc::JointVelocityLowerLimit, and robotoc::JointVelocityUpperLimit.
|
pure virtual |
Computes the primal residual, residual in the complementary slackness, and the log-barrier function of the slack varible.
[in] | robot | Robot model. |
[in] | contact_status | Contact status. |
[in] | data | Constraint data. |
[in] | s | Split solution. |
Implemented in robotoc::ContactWrenchCone, robotoc::FrictionCone, robotoc::JointAccelerationLowerLimit, robotoc::JointAccelerationUpperLimit, robotoc::JointPositionLowerLimit, robotoc::JointPositionUpperLimit, robotoc::JointTorquesLowerLimit, robotoc::JointTorquesUpperLimit, robotoc::JointVelocityLowerLimit, and robotoc::JointVelocityUpperLimit.
|
pure virtual |
Computes the derivatives of the priaml residual, i.e., the Jacobian of the inequality constraint, and add the product of the Jacobian and the dual variable to the KKT residual. This function is always called just after evalConstraint().
[in] | robot | Robot model. |
[in] | contact_status | Contact status. |
[in] | data | Constraint data. |
[in] | s | Split solution. |
[out] | kkt_residual | Split KKT residual. |
Implemented in robotoc::ContactWrenchCone, robotoc::FrictionCone, robotoc::JointAccelerationLowerLimit, robotoc::JointAccelerationUpperLimit, robotoc::JointPositionLowerLimit, robotoc::JointPositionUpperLimit, robotoc::JointTorquesLowerLimit, robotoc::JointTorquesUpperLimit, robotoc::JointVelocityLowerLimit, and robotoc::JointVelocityUpperLimit.
|
pure virtual |
Expands the slack and dual, i.e., computes the directions of the slack and dual variables from the directions of the primal variables.
[in] | contact_status | Contact status. |
[in,out] | data | Constraint data. |
[in] | d | Split direction. |
Implemented in robotoc::ContactWrenchCone, robotoc::FrictionCone, robotoc::JointAccelerationLowerLimit, robotoc::JointAccelerationUpperLimit, robotoc::JointPositionLowerLimit, robotoc::JointPositionUpperLimit, robotoc::JointTorquesLowerLimit, robotoc::JointTorquesUpperLimit, robotoc::JointVelocityLowerLimit, and robotoc::JointVelocityUpperLimit.
|
finalvirtual |
Gets the barrier parameter.
|
finalvirtual |
Gets the margin parameter of the fraction-to-boundary-rule.
|
pure virtual |
Checks whether the current solution s is feasible or not.
[in] | robot | Robot model. |
[in] | contact_status | Contact status. |
[in] | data | Constraint data. |
[in] | s | Split solution. |
Implemented in robotoc::ContactWrenchCone, robotoc::FrictionCone, robotoc::JointAccelerationLowerLimit, robotoc::JointAccelerationUpperLimit, robotoc::JointPositionLowerLimit, robotoc::JointPositionUpperLimit, robotoc::JointTorquesLowerLimit, robotoc::JointTorquesUpperLimit, robotoc::JointVelocityLowerLimit, and robotoc::JointVelocityUpperLimit.
|
pure virtual |
Checks the kinematics level of the constraint component.
Implemented in robotoc::ContactWrenchCone, robotoc::FrictionCone, robotoc::JointAccelerationLowerLimit, robotoc::JointAccelerationUpperLimit, robotoc::JointPositionLowerLimit, robotoc::JointPositionUpperLimit, robotoc::JointTorquesLowerLimit, robotoc::JointTorquesUpperLimit, robotoc::JointVelocityLowerLimit, and robotoc::JointVelocityUpperLimit.
|
inlineprotected |
Computes the log barrier function of the slack variable.
[in] | slack | Slack variable. All the components must be positive. |
|
inline |
Computes and returns the maximum step size by applying fraction-to-boundary-rule to the direction of the dual variable.
[in] | data | Constraint data. |
|
inline |
Computes and returns the maximum step size by applying fraction-to-boundary-rule to the direction of the slack variable.
[in] | data | Constraint data. |
|
default |
Default copy operator.
|
defaultnoexcept |
Default move assign operator.
|
finalvirtual |
Sets the barrier parameter.
[in] | barrier_param | Barrier parameter. Must be positive. Should be small. |
|
finalvirtual |
Sets the margin parameter of the fraction-to-boundary-rule.
[in] | fraction_to_boundary_rule | Must be larger than 0 and smaller than 1. Should be between 0.9 and 0.995. |
|
pure virtual |
Sets the slack variables of each constraint components.
[in] | robot | Robot model. |
[in] | contact_status | Contact status. |
[out] | data | Constraint data. |
[in] | s | Split solution. |
Implemented in robotoc::ContactWrenchCone, robotoc::FrictionCone, robotoc::JointAccelerationLowerLimit, robotoc::JointAccelerationUpperLimit, robotoc::JointPositionLowerLimit, robotoc::JointPositionUpperLimit, robotoc::JointTorquesLowerLimit, robotoc::JointTorquesUpperLimit, robotoc::JointVelocityLowerLimit, and robotoc::JointVelocityUpperLimit.
void robotoc::ConstraintComponentBase::setSlackAndDualPositive | ( | ConstraintComponentData & | data | ) | const |
Sets the slack and dual variables positive.
[in,out] | data | Constraint data. |
|
inlinestatic |
Updates the dual variable according to the step size.
[in,out] | data | Constraint data. |
[in] | step_size | Step size. |
|
inlinestatic |
Updates the slack variable according to the step size.
[in,out] | data | Constraint data. |
[in] | step_size | Step size. |