Problem Formulation¶

The following classes make up the problem formulation.

Nlp Formulation¶

namespace gambol

class NlpFormulation¶

#include <NlpFormulation.h>

A combination of variablesets and constraints

This class is basically a factory for NLP, to be solved by IFOPT.

Public Types

using VariablePtrVec = std::vector<ifopt::VariableSet::Ptr>¶

using ContraintPtrVec = std::vector<ifopt::ConstraintSet::Ptr>¶

using CostPtrVec = std::vector<ifopt::CostTerm::Ptr>¶

using VectorXd = Eigen::VectorXd¶

using Vector3d = Eigen::Vector3d¶

using Vector4d = Eigen::Vector4d¶

Public Functions

NlpFormulation()¶: Constructor

virtual ~NlpFormulation() = default¶: Destructor

VariablePtrVec GetVariableSets(NodesHolder &nodes_holder)¶: Get variables to be optimised over

ContraintPtrVec GetConstraints(const NodesHolder &nodes_holder) const¶: Return set of all constraints

ContraintPtrVec GetCosts() const¶: Return set of all costs

Public Members

Parameters params_¶: Parameters of optimisation

RobotModel::Ptr model_¶: Underlying robot model

HeightMap::Ptr terrain_¶: Terrain object

int size_q_¶: Number of states.

int size_dq_¶: Number of velocity/acceleration states.

int size_u_¶: Number of torque inputs.

VectorXd initial_joint_pos_¶

VectorXd initial_joint_vel_¶: Initial joint position.

VectorXd final_joint_pos_¶

VectorXd final_joint_vel_¶: Final joint position.

std::vector<int> bound_initial_joint_pos_¶

std::vector<int> bound_initial_joint_vel_¶: Indices of joints to bound.

std::vector<int> bound_final_joint_pos_¶

std::vector<int> bound_final_joint_vel_¶: Indices of joints to bound.

std::vector<std::pair<double, double>> joint_pos_limits_¶: Joint limits.

std::vector<int> bound_joint_pos_limits_¶: Indices of joints which to limit.

std::vector<std::pair<double, double>> torque_limits_¶: Torque limits.

std::vector<int> bound_torque_limits_¶: Indices of torques to limit.

std::vector<int> bound_symmetry_joint_pos_¶: Indices of joint to fix for symmetry.

bool initial_zpos_¶: True of initial z-pos is bound.

NodeTimes::Ptr node_times_¶: Original node_times object.

std::vector<PhaseDurations::Ptr> phase_durations_¶: Original phases object.

Public Static Functions

static void setSolverOptions(const ifopt::IpoptSolver::Ptr &solver)¶

Set solver options

max_iter and max_cpu_time still need to be set outside this function.

tol - Desired convergence tolerance, scaled NLP error must be less than this constr_viol_tol - Max. unscaled constr. violation must be less than this dual_inf_tol - Max. unscaled dual infeasibility must be less than this compl_inf_tol - Max. unscaled complementarity must be less than this

acceptable_* - Same as above but for acceptable termination

Private Functions

std::vector<NodesVariables::Ptr> MakeVariablesJoints() const¶: Return joint variables for position and velocity

NodesVariables::Ptr MakeVariablesTorques() const¶: Return joint variables for joint torques

std::vector<NodesVariables::Ptr> MakeVariablesForces() const¶

Return joint variables for end-effector forces

Return vector with variable for each end-effector.

NodeTimes::Ptr MakeNodeTimes() const¶: Return shared NodeTimes object

std::vector<PhaseDurations::Ptr> MakePhaseDurations() const¶: Return shared PhaseDurations object

ContraintPtrVec GetConstraint(Parameters::ConstraintName name, const NodesHolder &splines) const¶: Get specific constraint through name

ContraintPtrVec MakeConstraintIntegration(const NodesHolder &s) const¶: Get velocity integration constraint

ContraintPtrVec MakeConstraintQuaternions(const NodesHolder &s) const¶: Get quaternion constraint

ContraintPtrVec MakeConstraintDynamics(const NodesHolder &s) const¶: Get velocity integration constraint

ContraintPtrVec MakeConstraintTerrain(const NodesHolder &s) const¶: Get terrain constraint

ContraintPtrVec MakeConstraintForces(const NodesHolder &s) const¶: Get forces constraint

ContraintPtrVec MakeConstraintSymmetry(const NodesHolder &s) const¶

Get symmetry constraint

Use bound_symmetry_joint_pos_ to determine which positions should be constraint. All velocities and torques are constraint by default.

CostPtrVec GetCost(const Parameters::CostName &id, double weight) const¶: Get a single cost based on name and weight

CostPtrVec MakeCostTorque(double weight) const¶: Make torque cost

CostPtrVec MakeCostJointAcceleration(double weight) const¶: Make joint acceleration cost

CostPtrVec MakeCostAngularVelocity(double weight) const¶: Make cost for angular velocity of main body

CostPtrVec MakeCostFootLift(double weight) const¶: Make foot-lift cost

void SetJointsInitialGuess(NodesHolder &nodes_holder) const¶

Set joint initial guess

Based on initial and final positions and robot model.

Parameters¶

namespace gambol

class Parameters¶

#include <Parameters.h>

Parameters to control the optimisation

Public Types

enum ConstraintName¶

Values:

enumerator Integration¶: Velocity to position integration.

enumerator Quaternions¶: Unity quaternion constraint.

enumerator Dynamics¶: Acceleration integration.

enumerator Terrain¶: Terrain constraint.

enumerator Forces¶: Forces constraint.

enumerator Symmetry¶: Make last pose equal to initial pose.

enum CostName¶

Values:

enumerator Torque¶: Cost on torque nodes.

enumerator JointAcceleration¶: Cost on joint accelerations.

enumerator AngularVelocity¶: Angular velocity of base.

enumerator FootLiftReward¶: Award lifting the foot above the ground.

using VecTimes = std::vector<double>¶

using UsedConstraints = std::vector<ConstraintName>¶

using UsedCosts = std::vector<std::pair<CostName, double>>¶

Public Functions

Parameters()¶: Constructor

virtual ~Parameters() = default¶: Destructor

double GetTotalTime() const¶

Return sum of phase durations

Also assert the sum for each end-effector is equal

uint GetEECount() const¶: Return number of end-effectors

Public Members

UsedConstraints constraints_¶: Names of constraints to be used.

UsedCosts costs_¶: Names of costs to be used.

int N_¶: Number of collocation points.

double t_total¶: Total duration of optimisation.

std::vector<bool> initial_contact_¶

std::vector<VecTimes> ee_phase_durations_¶: Phase duration of each end-effector.

double max_normal_force_¶

bool symmetry_¶

Public Static Functions

static std::vector<int> GetLinspaceVector(int n, int start = 0, int step = 1)¶: Return linspaced integer vector

Saving NLP to file¶

namespace gambol

class NLPToFile¶

#include <NLPToFile.h>

Little helper class to for saving a NLP to file

The class can save the optimization outcome to a file, and use such a previous outcome for a next optimization (iff the NLP is the same).

Public Functions

NLPToFile(ifopt::Problem *nlp, const std::string &filename)¶: Constructor

virtual ~NLPToFile() = default¶

bool SetGuessFromFile()¶: Set guess form file

bool WriteFileFromResult() const¶: Write current variable values of NLP to file

Private Members

std::string filename_¶

ifopt::Problem *nlp_¶