humoto
|
base CoM tracking
Definition at line 111 of file task_basecom.h.
#include <task_basecom.h>
Public Member Functions | |
TaskBaseCoMTracking (const double gain=1.0, const double k_position_gain=1.0) | |
void | addATAandATb (Eigen::MatrixXd &H, Eigen::VectorXd &g) const |
Compute 'A^T * A' for general equality constaints and save or add the result to H. Compute 'A^T * b' for general equality constaints and save or add the result to g. More... | |
void | checkConsistency () const |
Check consistency of the constraints. More... | |
void | computeViolations (ViolationsConstraints &violations, const Location &location, const Solution &solution) const |
Compute violations given a solution vector. More... | |
void | copyBodyTo (Eigen::MatrixXd &A, const Location &location) const |
Copy body to the given matrix. More... | |
void | copyBoundsTo (Eigen::VectorXd &lb, Eigen::VectorXd &ub, const Location &location) const |
Copy bounds to given vectors. More... | |
void | copyNegativeBodyTo (Eigen::MatrixXd &A, const Location &location) const |
Copy body to the given matrix. More... | |
virtual std::size_t | copyTo (ContainerILU &container, const std::size_t constraints_offset) const |
Set constraints. More... | |
virtual std::size_t | copyTo (ContainerAL &container, const std::size_t constraints_offset) const |
Copy constraints to a container. More... | |
std::size_t | copyTo (ContainerALU &container, const std::size_t constraints_offset) const |
Set constraints. More... | |
std::size_t | copyTo (ContainerAB &container, const std::size_t constraints_offset) const |
Set constraints. More... | |
void | determineActiveSet (ActiveSetConstraints &active_set, const Location &location, const Solution &solution) const |
Determine active set given a solution vector. More... | |
void | form (const humoto::SolutionStructure &sol_structure, const humoto::Model &model_base, const humoto::ControlProblem &control_problem) |
Form the task. More... | |
Eigen::MatrixXd & | getA () |
Get matrix A from general constraints: 'A*x = b', 'lb <= A*x <= ub'. More... | |
const Eigen::MatrixXd & | getA () const |
Get matrix A from general constraints: 'A*x = b', 'lb <= A*x <= ub'. More... | |
void | getATAandATb (Eigen::MatrixXd &H, Eigen::VectorXd &g) const |
Compute 'A^T * A' for general equality constaints and save or add the result to H. Compute 'A^T * b' for general equality constaints and save or add the result to g. More... | |
Eigen::VectorXd & | getB () |
Get vector b from equalities: 'A*x = b' or 'x = b'. More... | |
const Eigen::VectorXd & | getB () const |
Get vector b from equalities: 'A*x = b' or 'x = b'. More... | |
Location | getCopyLocation (const std::size_t number_of_constraints, const std::size_t constraints_offset, const std::size_t number_of_variables) const |
Computes location of the copied constraints in a container and verifies their consistency. More... | |
const std::string | getDescription () const |
Get description of the task. More... | |
double | getGain () const |
Returns gain. More... | |
std::size_t | getNumberOfConstraints () const |
Returns number of constraints in the task. More... | |
std::size_t | getNumberOfVariables () const |
Returns number of variables in the task. More... | |
double | getProduct (const std::size_t i, const Eigen::VectorXd &vector) const |
Compute A(i,:) * vector. More... | |
ConstraintType::Type | getType () const |
Returns type of the constraints. More... | |
bool | isEquality () const |
Check properties of the constriaints. More... | |
bool | isSimple () const |
Check properties of the constriaints. More... | |
bool | isTwoSidedInequality () const |
Check properties of the constriaints. More... | |
void | log (humoto::Logger &logger, const LogEntryName &parent=LogEntryName(), const std::string &name="task") const |
Log task. More... | |
void | readConfig (t_Reader &reader, const bool crash_on_missing_entry=default_crash_on_missing_entry_) |
Read configuration (assuming the configuration node to be in the root). More... | |
void | readConfig (t_Reader &reader, const std::string &node_name, const bool crash_on_missing_entry=default_crash_on_missing_entry_) |
Read configuration (assuming the configuration node to be in the root). More... | |
void | readConfig (t_Reader &reader, const char *node_name, const bool crash_on_missing_entry=default_crash_on_missing_entry_) |
Read configuration (assuming the configuration node to be in the root). More... | |
void | readConfig (const std::string &file_name, const bool crash_on_missing_entry=default_crash_on_missing_entry_) |
Read configuration (assuming the configuration node to be in the root). More... | |
void | readConfig (const std::string &file_name, const std::string &node_name, const bool crash_on_missing_entry=default_crash_on_missing_entry_) |
Read configuration (assuming the configuration node to be in the root). More... | |
void | readConfig (const std::string &file_name, const char *node_name, const bool crash_on_missing_entry=default_crash_on_missing_entry_) |
Read configuration (assuming the configuration node to be in the root). More... | |
void | readNestedConfig (t_Reader &reader, const std::string &node_name, const bool crash_on_missing_entry=default_crash_on_missing_entry_) |
Read nested configuration node. More... | |
void | setDescription (const std::string &description) |
Set description of the task. More... | |
void | setGain (const double gain) |
Sets gain. More... | |
void | writeConfig (t_Writer &writer) const |
Write configuration. More... | |
void | writeConfig (t_Writer &writer, const std::string &node_name) const |
Write configuration. More... | |
void | writeConfig (const std::string &file_name) const |
Write configuration. More... | |
void | writeConfig (const std::string &file_name, const std::string &node_name) const |
Write configuration. More... | |
void | writeNestedConfig (t_Writer &writer) const |
Write nested configuration node. More... | |
void | writeNestedConfig (t_Writer &writer, const std::string &node_name) const |
Write nested configuration node. More... | |
Protected Member Functions | |
void | addATA (Eigen::MatrixXd &H) const |
Compute 'A^T * A' for general equality constaints and save or add the result to H. More... | |
void | copyEqualityBoundsTo (Eigen::VectorXd &b, const Location &location) const |
Copy b. More... | |
void | finalize () |
ActiveSetConstraints & | getActiveSetGuess () |
Get active set guess. More... | |
const ActiveSetConstraints & | getActualActiveSet () const |
Get actual active set. More... | |
void | getATA (Eigen::MatrixXd &H) const |
Compute 'A^T * A' for general equality constaints and save or add the result to H. More... | |
const std::string & | getConfigSectionID () const |
Return the default name of a configuration node corresponding to this class. More... | |
virtual std::size_t | getNumberOfEntries () const=0 |
Get number of entries in the corresponding configuration node. More... | |
virtual void | guessActiveSet (const humoto::SolutionStructure &sol_structure, const humoto::Model &model_base, const humoto::ControlProblem &control_problem) |
Initialize active set guess with defaults. More... | |
void | logBody (humoto::Logger &logger, const LogEntryName &parent=LogEntryName(), const std::string &name="constraints") const |
Log body. More... | |
void | logBounds (humoto::Logger &logger, const LogEntryName &parent=LogEntryName(), const std::string &name="constraints") const |
Log bounds. More... | |
virtual void | logTask (humoto::Logger &logger, const LogEntryName &parent=LogEntryName(), const std::string &name="task") const |
Log task. More... | |
void | markAsUnmodified () |
By default the task is assumed to be modified. Use this function to mark it as unmodified to avoid unnecessary recomputations. More... | |
void | reset (const std::size_t number_of_constraints=0, const std::size_t number_of_variables=0, const bool initialize_matrices=false) |
Initialize constraints. More... | |
void | resetBody (const std::size_t number_of_constraints, const bool initialize_matrices=false) |
Initialize A. More... | |
void | resetBounds (const std::size_t number_of_constraints=0, const bool initialize_matrices=false) |
Reset bounds of constraints (b, lb, ub) More... | |
virtual void | setDefaults () |
Set members to their default values. More... | |
void | setNumberOfVariables (const std::size_t number_of_variables) |
Set number of variables. More... | |
Protected Attributes | |
Eigen::MatrixXd | A_ |
ActiveSetConstraints | active_set_actual_ |
Eigen::VectorXd | b_ |
double | gain_ |
bool | is_modified_ |
double | k_position_gain_ |
std::size_t | number_of_variables_ |
Static Protected Attributes | |
static const bool | default_crash_on_missing_entry_ |
|
inline |
Definition at line 121 of file task_basecom.h.
|
inlineprotectedinherited |
Compute 'A^T * A' for general equality constaints and save or add the result to H.
[in,out] | H | left lower triangular part of the result. |
Definition at line 503 of file constraints_mixins.h.
|
inlinevirtualinherited |
Compute 'A^T * A' for general equality constaints and save or add the result to H. Compute 'A^T * b' for general equality constaints and save or add the result to g.
[in,out] | H | left lower triangular part of the result. |
[in,out] | g | result |
Reimplemented from humoto::constraints::ConstraintsBase.
Definition at line 211 of file constraints.h.
|
inlinevirtualinherited |
Check consistency of the constraints.
Implements humoto::constraints::ConstraintsBase.
Definition at line 192 of file constraints.h.
|
inlinevirtualinherited |
Compute violations given a solution vector.
[in,out] | violations | violations |
[in] | location | location of the constraints in the active set |
[in] | solution | solution vector |
Implements humoto::constraints::ConstraintsBase.
Definition at line 1333 of file constraints_mixins.h.
|
inlinevirtualinherited |
Copy body to the given matrix.
[in] | A | |
[in] | location | offset and length |
Implements humoto::constraints::ConstraintsBase.
Definition at line 530 of file constraints_mixins.h.
|
inlinevirtualinherited |
Copy bounds to given vectors.
[in] | lb | |
[in] | ub | |
[in] | location | offset and length |
Implements humoto::constraints::ConstraintsBase.
Definition at line 190 of file constraints_mixins.h.
|
inlineprotectedinherited |
|
inlinevirtualinherited |
Copy body to the given matrix.
[in] | A | |
[in] | location | offset and length |
Implements humoto::constraints::ConstraintsBase.
Definition at line 540 of file constraints_mixins.h.
|
inlinevirtualinherited |
Set constraints.
[in,out] | container | new constraints |
[in] | constraints_offset | first row offset |
Reimplemented in humoto::constraints::CopySimpleToILUMixin< CopyEqualityToABMixin< ViolationsComputationBMixin< ActiveSetDeterminationBMixin< BodyIMixin< BoundsBMixin< TaskBase > > > > > >, humoto::constraints::CopySimpleToILUMixin< ViolationsComputationLUMixin< ActiveSetDeterminationLUMixin< BodyIMixin< BoundsLUMixin< TaskBase > > > > >, humoto::constraints::CopySimpleToILUMixin< ViolationsComputationLMixin< ActiveSetDeterminationLMixin< BodyIMixin< BoundsLMixin< TaskBase > > > > >, humoto::constraints::CopySimpleToILUMixin< ViolationsComputationUMixin< ActiveSetDeterminationUMixin< BodyIMixin< BoundsUMixin< TaskBase > > > > >, and humoto::constraints::CopySimpleToILUMixin< CopyEqualityToABMixin< ViolationsComputationB0Mixin< ActiveSetDeterminationBMixin< BodyIMixin< BoundsB0Mixin< TaskBase > > > > > >.
Definition at line 457 of file constraints_base.h.
|
inlinevirtualinherited |
Copy constraints to a container.
[in,out] | container | |
[in] | constraints_offset | first row offset |
Reimplemented in humoto::constraints::CopyTwoSidedInequalityToALMixin< CopyAnyToALUMixin< ViolationsComputationLUMixin< ActiveSetDeterminationLUMixin< BodyASMixin< BoundsLUMixin< TaskBase > > > > > >, humoto::constraints::CopyTwoSidedInequalityToALMixin< CopyAnyToALUMixin< CopySimpleToILUMixin< ViolationsComputationLUMixin< ActiveSetDeterminationLUMixin< BodyIMixin< BoundsLUMixin< TaskBase > > > > > > >, humoto::constraints::CopyTwoSidedInequalityToALMixin< CopyAnyToALUMixin< ViolationsComputationLUMixin< ActiveSetDeterminationLUMixin< BodyGIMixin< BoundsLUMixin< TaskBase > > > > > >, humoto::constraints::CopyTwoSidedInequalityToALMixin< CopyAnyToALUMixin< ViolationsComputationLUMixin< ActiveSetDeterminationLUMixin< BodyAMixin< BoundsLUMixin< TaskBase > > > > > >, humoto::constraints::CopyUpperInequalityToALMixin< CopyLowerInequalityToALMixin< CopyAnyToALUMixin< ViolationsComputationLUMixin< ActiveSetDeterminationLUMixin< BodyGIMixin< BoundsLUMixin< TaskBase > > > > > > >, humoto::constraints::CopyUpperInequalityToALMixin< CopyAnyToALUMixin< ViolationsComputationUMixin< ActiveSetDeterminationUMixin< BodyAMixin< BoundsUMixin< TaskBase > > > > > >, humoto::constraints::CopyUpperInequalityToALMixin< CopyLowerInequalityToALMixin< CopyAnyToALUMixin< ViolationsComputationLUMixin< ActiveSetDeterminationLUMixin< BodyASMixin< BoundsLUMixin< TaskBase > > > > > > >, humoto::constraints::CopyUpperInequalityToALMixin< CopyLowerInequalityToALMixin< CopyAnyToALUMixin< CopySimpleToILUMixin< ViolationsComputationLUMixin< ActiveSetDeterminationLUMixin< BodyIMixin< BoundsLUMixin< TaskBase > > > > > > > >, humoto::constraints::CopyUpperInequalityToALMixin< CopyAnyToALUMixin< ViolationsComputationUMixin< ActiveSetDeterminationUMixin< BodyASMixin< BoundsUMixin< TaskBase > > > > > >, humoto::constraints::CopyUpperInequalityToALMixin< CopyLowerInequalityToALMixin< CopyAnyToALUMixin< ViolationsComputationLUMixin< ActiveSetDeterminationLUMixin< BodyAMixin< BoundsLUMixin< TaskBase > > > > > > >, humoto::constraints::CopyUpperInequalityToALMixin< CopyAnyToALUMixin< ViolationsComputationUMixin< ActiveSetDeterminationUMixin< BodyGIMixin< BoundsUMixin< TaskBase > > > > > >, humoto::constraints::CopyUpperInequalityToALMixin< CopyAnyToALUMixin< CopySimpleToILUMixin< ViolationsComputationUMixin< ActiveSetDeterminationUMixin< BodyIMixin< BoundsUMixin< TaskBase > > > > > > >, humoto::constraints::CopyLowerInequalityToALMixin< CopyAnyToALUMixin< ViolationsComputationLMixin< ActiveSetDeterminationLMixin< BodyAMixin< BoundsLMixin< TaskBase > > > > > >, humoto::constraints::CopyLowerInequalityToALMixin< CopyAnyToALUMixin< ViolationsComputationLUMixin< ActiveSetDeterminationLUMixin< BodyASMixin< BoundsLUMixin< TaskBase > > > > > >, humoto::constraints::CopyLowerInequalityToALMixin< CopyAnyToALUMixin< CopySimpleToILUMixin< ViolationsComputationLUMixin< ActiveSetDeterminationLUMixin< BodyIMixin< BoundsLUMixin< TaskBase > > > > > > >, humoto::constraints::CopyLowerInequalityToALMixin< CopyAnyToALUMixin< CopySimpleToILUMixin< ViolationsComputationLMixin< ActiveSetDeterminationLMixin< BodyIMixin< BoundsLMixin< TaskBase > > > > > > >, humoto::constraints::CopyLowerInequalityToALMixin< CopyAnyToALUMixin< ViolationsComputationLMixin< ActiveSetDeterminationLMixin< BodyASMixin< BoundsLMixin< TaskBase > > > > > >, humoto::constraints::CopyLowerInequalityToALMixin< CopyAnyToALUMixin< ViolationsComputationLUMixin< ActiveSetDeterminationLUMixin< BodyGIMixin< BoundsLUMixin< TaskBase > > > > > >, humoto::constraints::CopyLowerInequalityToALMixin< CopyAnyToALUMixin< ViolationsComputationLMixin< ActiveSetDeterminationLMixin< BodyGIMixin< BoundsLMixin< TaskBase > > > > > >, and humoto::constraints::CopyLowerInequalityToALMixin< CopyAnyToALUMixin< ViolationsComputationLUMixin< ActiveSetDeterminationLUMixin< BodyAMixin< BoundsLUMixin< TaskBase > > > > > >.
Definition at line 472 of file constraints_base.h.
|
inlinevirtualinherited |
Set constraints.
[in,out] | container | new constraints |
[in] | constraints_offset | first row offset |
Reimplemented from humoto::constraints::ConstraintsBase.
Definition at line 615 of file constraints_base.h.
|
inlinevirtualinherited |
Set constraints.
[in,out] | container | new constraints |
[in] | constraints_offset | first row offset |
Reimplemented from humoto::constraints::ConstraintsBase.
Definition at line 777 of file constraints_base.h.
|
inlinevirtualinherited |
Determine active set given a solution vector.
[in,out] | active_set | active set |
[in] | location | location of the constraints in the active set |
[in] | solution | solution vector |
Implements humoto::constraints::ConstraintsBase.
Definition at line 1187 of file constraints_mixins.h.
|
inlineprotectedvirtualinherited |
Reimplemented from humoto::config::CommonConfigurableBase< true >.
|
inlinevirtual |
Form the task.
[in] | sol_structure | structure of the solution |
[in] | model_base | model |
[in] | control_problem | control problem |
Implements humoto::TaskBase.
Definition at line 128 of file task_basecom.h.
|
inlineinherited |
Get matrix A from general constraints: 'A*x = b', 'lb <= A*x <= ub'.
Definition at line 517 of file constraints_mixins.h.
|
inlineinherited |
Get matrix A from general constraints: 'A*x = b', 'lb <= A*x <= ub'.
Definition at line 523 of file constraints_mixins.h.
|
inlineprotectedinherited |
|
inlineprotectedinherited |
|
inlineprotectedinherited |
Compute 'A^T * A' for general equality constaints and save or add the result to H.
[in,out] | H | left lower triangular part of the result. |
Definition at line 495 of file constraints_mixins.h.
|
inlinevirtualinherited |
Compute 'A^T * A' for general equality constaints and save or add the result to H. Compute 'A^T * b' for general equality constaints and save or add the result to g.
[in,out] | H | left lower triangular part of the result. |
[in,out] | g | result |
Reimplemented from humoto::constraints::ConstraintsBase.
Definition at line 203 of file constraints.h.
|
inlineinherited |
Get vector b from equalities: 'A*x = b' or 'x = b'.
Definition at line 176 of file constraints_mixins.h.
|
inlineinherited |
Get vector b from equalities: 'A*x = b' or 'x = b'.
Definition at line 183 of file constraints_mixins.h.
|
inlineprotectedvirtualinherited |
Return the default name of a configuration node corresponding to this class.
Implements humoto::config::CommonConfigurableBase< true >.
|
inlineinherited |
Computes location of the copied constraints in a container and verifies their consistency.
[in] | number_of_constraints | number of added constraints |
[in] | constraints_offset | offset in the container |
[in] | number_of_variables | number of variables in the container |
Definition at line 262 of file constraints_base.h.
|
inlineinherited |
|
inlineinherited |
|
inlinevirtualinherited |
Returns number of constraints in the task.
Implements humoto::constraints::ContainerBase.
Definition at line 550 of file constraints_mixins.h.
|
protectedpure virtualinherited |
Get number of entries in the corresponding configuration node.
Implemented in humoto::ConfigurableOptimizationProblem.
|
inlineinherited |
Returns number of variables in the task.
Definition at line 215 of file constraints_base.h.
|
inlineinherited |
Compute A(i,:) * vector.
[in] | i | row index |
[in] | vector | vector |
Definition at line 564 of file constraints_mixins.h.
|
inlinevirtualinherited |
Returns type of the constraints.
Implements humoto::constraints::ConstraintsBase.
Definition at line 185 of file constraints.h.
|
inlineprotectedvirtualinherited |
Initialize active set guess with defaults.
[in] | sol_structure | structure of the solution |
[in] | model_base | model |
[in] | control_problem | control problem |
Reimplemented in humoto::wpg04::TaskFootstepBounds, humoto::wpg04::TaskCoPBounds, humoto::pepper_mpc::TaskBaseAccelerationBounds, humoto::pepper_mpc::TaskBaseVelocityBounds, humoto::pepper_mpc::TaskCoPPositionBounds, and humoto::pepper_mpc::TaskBodyPositionBounds.
|
inlineinherited |
Check properties of the constriaints.
Definition at line 523 of file constraints_base.h.
|
inlineinherited |
Check properties of the constriaints.
Definition at line 529 of file constraints_base.h.
|
inlineinherited |
Check properties of the constriaints.
Definition at line 535 of file constraints_base.h.
|
inlineinherited |
|
inlineprotectedvirtualinherited |
Log body.
[in,out] | logger | logger |
[in] | parent | parent |
[in] | name | name |
Implements humoto::constraints::ContainerBase.
Definition at line 476 of file constraints_mixins.h.
|
inlineprotectedvirtualinherited |
Log bounds.
[in,out] | logger | logger |
[in] | parent | parent |
[in] | name | name |
Implements humoto::constraints::ContainerBase.
Definition at line 149 of file constraints_mixins.h.
|
inlineprotectedvirtualinherited |
Log task.
[in] | logger | logger |
[in] | parent | parent |
[in] | name | name |
Reimplemented from humoto::GeneralTaskBaseMixin< constraints::ConstraintsAB< TaskBase > >.
Definition at line 54 of file task_basecom.h.
|
inlineprotectedinherited |
By default the task is assumed to be modified. Use this function to mark it as unmodified to avoid unnecessary recomputations.
|
inlineinherited |
|
inlineinherited |
|
inlineinherited |
|
inlineinherited |
|
inlineinherited |
|
inlineinherited |
|
inlineinherited |
|
inlineprotectedinherited |
Initialize constraints.
[in] | number_of_constraints | number of constraints |
[in] | number_of_variables | number of variables |
[in] | initialize_matrices | initialize matrices using defaults |
Definition at line 175 of file constraints_base.h.
|
inlineprotectedvirtualinherited |
Initialize A.
[in] | number_of_constraints | number of constraints |
[in] | initialize_matrices | initialize matrices using defaults |
Initialization:
Implements humoto::constraints::ContainerBase.
Definition at line 462 of file constraints_mixins.h.
|
inlineprotectedvirtualinherited |
Reset bounds of constraints (b, lb, ub)
[in] | number_of_constraints | number of constraints |
[in] | initialize_matrices | initialize matrices using defaults |
Implements humoto::constraints::ContainerBase.
Definition at line 134 of file constraints_mixins.h.
|
inlineprotectedvirtualinherited |
Set members to their default values.
Reimplemented from humoto::GeneralTaskBaseMixin< constraints::ConstraintsAB< TaskBase > >.
Definition at line 40 of file task_basecom.h.
|
inlineinherited |
|
inlineinherited |
|
inlineprotectedinherited |
Set number of variables.
[in] | number_of_variables | number of variables |
Definition at line 142 of file constraints_base.h.
|
inlineinherited |
|
inlineinherited |
|
inlineinherited |
|
inlineinherited |
|
inlineinherited |
|
inlineinherited |
|
protectedinherited |
Definition at line 441 of file constraints_mixins.h.
|
protectedinherited |
|
protectedinherited |
Definition at line 118 of file constraints_mixins.h.
|
staticprotectedinherited |
|
protectedinherited |
|
protectedinherited |
Definition at line 28 of file task_basecom.h.
|
protectedinherited |
Definition at line 120 of file constraints_base.h.