Constraints¶

Constraints in the NLP are created through the following classes.

Nodes Constraint¶

namespace gambol¶

class NodesConstraint : public ConstraintSet¶

#include <NodesConstraint.h>

Constraints evaluated at each node in a trajectory

Subclassed by gambol::DynamicsConstraint, gambol::ForceConstraint, gambol::ForceFlatConstraint, gambol::IntegrationConstraint, gambol::QuaternionConstraint, gambol::TerrainConstraint, gambol::TerrainFlatConstraint

Public Types

using VectorXd = Eigen::VectorXd¶

using Vector3d = Eigen::Vector3d¶

using MatrixXd = Eigen::MatrixXd¶

using VecTimes = std::vector<double>¶

using Bounds = ifopt::Bounds¶

Public Functions

VectorXd GetValues() const override¶: Return vector of constraint values

VecBound GetBounds() const override¶: Return vector of constraint bounds

void FillJacobianBlock(std::string var_set, Jacobian&) const override¶: Return complete constraint jacobian

Protected Functions

NodesConstraint(const NodesHolder &s, const std::string &name)¶: Constructor

virtual ~NodesConstraint() = default¶

int GetNumberOfNodes() const¶: Return number of nodes

Protected Attributes

NodesHolder nodes_holder_¶

Private Functions

virtual void UpdateConstraintAtNode(int k, VectorXd &g) const = 0¶

Sets the constraint value a node k.

Parameters

k: The index of the time t, so t=k*dt
[inout] g: The complete vector of constraint values, for which the corresponding row must be filled.

virtual void UpdateBoundsAtNode(int k, VecBound &b) const = 0¶

Sets upper/lower bound to node k.

Parameters

k: The index of the time t, so t=k*dt
[inout] b: The complete vector of bounds, for which the corresponding row must be set.

virtual void UpdateJacobianAtNode(int k, std::string var_set, Jacobian &jac) const = 0¶

Sets Jacobian rows at a specific node k.

Parameters

k: The index of the time t, so t=k*dt
var_set: The name of the ifopt variables currently being queried for.
[inout] jac: The complete Jacobian, for which the corresponding row and columns must be set.

Dynamics¶

namespace gambol

class DynamicsConstraint : public gambol::NodesConstraint ¶

#include <DynamicsConstraint.h>

Constraint to keep dynamics correct

Dynamics is computed as:

    ddq[k] = dynamics[k] = M[k]^-1 * net_torque[k]

Constraint value is integrated velocity violation:

    dq[k] - dq[k+1] + dt/2 * (ddq[k] + ddq[k+1])

Public Functions

DynamicsConstraint(const RobotModel::Ptr &model, const NodesHolder &nodes_holder)¶: Constructor

virtual ~DynamicsConstraint() = default¶

Private Functions

int GetRow(int k, int dim = 0) const¶

Return index of constraint based on node info

Get row of constraint vector based on node info

bool Update(int k) const¶

Extract states for current node

Update state properties for current node

Return: bool False if k is the last node

virtual void UpdateConstraintAtNode(int k, VectorXd &g) const override¶: Update constraint value at node

virtual void UpdateBoundsAtNode(int k, VecBound &b) const override¶: Set bound for specific node

virtual void UpdateJacobianAtNode(int k, std::string var_set, Jacobian &jac) const override¶

Set jacobian for this node

jac is the jacobian of the entire constraint to the current variable set.

Private Members

mutable RobotModel::Ptr model_k_¶: Robot model on which dynamics is based.

mutable RobotModel::Ptr model_kp1_¶: Robot model on which dynamics is based.

int size_q_¶

int size_dq_¶

mutable double dt_k_¶

Integration¶

namespace gambol

class IntegrationConstraint : public gambol::NodesConstraint ¶

#include <IntegrationConstraint.h>

Constraint to ensure velocity between steps integrates to position

Constraint value is integration error:

    q[k] - q[k+1] + dt/2 * (dq[k] + dq[k+1])

Note that for some models v != dq/dt. Hence a jacobian is used from the robot model:

q[k] - q[k+1] + dt/2 * (B[k] * v[k] + B[k+1] * v[k+1])

Where

    dq(t)/dt = B(t) * v(t)

Public Types

using MatrixXd = Eigen::MatrixXd¶

Public Functions

IntegrationConstraint(const RobotModel::Ptr &model, const NodesHolder &nodes_holder)¶: Constructor

virtual ~IntegrationConstraint() = default¶

Private Functions

int GetRow(int k, int dim = 0) const¶

Return index of constraint based on node info

Get row of constraint vector based on node info

bool Update(int k) const¶

Extract states for current node

Update state properties for current node

Return: bool False if k is the last node

virtual void UpdateConstraintAtNode(int k, VectorXd &g) const override¶: Update constraint value at node

virtual void UpdateBoundsAtNode(int k, VecBound &b) const override¶: Set bound for specific node

virtual void UpdateJacobianAtNode(int k, std::string var_set, Jacobian &jac) const override¶

Set jacobian for this node

jac is the jacobian of the entire constraint to the current variable set.

Private Members

int size_q_¶

int size_dq_¶

mutable double dt_k_¶

mutable RobotModel::Ptr model_k_¶: Robot model on which dynamics is based.

mutable RobotModel::Ptr model_kp1_¶: Robot model on which dynamics is based.

Terrain¶

namespace gambol

class TerrainConstraint : public gambol::NodesConstraint ¶

#include <TerrainConstraint.h>

Terrain constraint

Keeps end-effector on the ground with velocity zero during stance and keeps it above ground during swing.

    | ' |   |   | ' |
    | ' |   |   | ' x
    x ' |   |   | ' |
    | ' |   |   | ' |
    | ' x   x   x ' |
    | ' |   |   | ' |
(sw)       (st)      (sw)

The constraints for each node are: z_ee_k x_ee_k - x_ee_k-1 y_ee_k - y_ee_k-1

During swing the bounds are (0,inf) and (-inf,inf) respectively. During stance the bounds are (0,0) for both. Except for the first node of a stance phase! For this node the x-position constraint is also (-inf,inf).

Public Types

using Vector3d = Eigen::Vector3d¶

Public Functions

TerrainConstraint(const HeightMap::Ptr &terrain, const RobotModel::Ptr &model, const NodesHolder &nodes_holder, uint ee_id, bool skip_initial = false)¶

Constructor

Parameters

terrain: Pointer to heightmap
model: Pointer to robot model
nodes_holder: Collection of variables
ee_id: End-effector this constraint applies to
skip_initial: Set to true to not constraint the first frame

virtual ~TerrainConstraint() = default¶

bool Update(int k) const¶

Update state properties for current node

Return: bool False if this k should be skipped

virtual void UpdateConstraintAtNode(int k, VectorXd &g) const override¶: Get constraint values

virtual void UpdateBoundsAtNode(int k, VecBound &b) const override¶: Get constraint bounds

virtual void UpdateJacobianAtNode(int k, std::string var_set, Jacobian &jac) const override¶: Fill jacobian for constraint

Private Functions

int GetRow(int k, int type) const¶: Get row of constraint vector based on node info

Private Members

uint ee_id_¶

bool skip_initial_¶

HeightMap::Ptr terrain_¶: Terrain used as height map.

mutable RobotModel::Ptr model_¶: Robot model used for kinematics.

mutable RobotModel::Ptr model_km1_¶

namespace gambol

class TerrainFlatConstraint : public gambol::NodesConstraint ¶

#include <TerrainFlatConstraint.h>

Terrain constraint

(This is an older version that does not incorporate terrain)

Keeps end-effector on the ground with velocity zero during stance and keeps it above ground during swing.

    | ' |   |   | ' |
    | ' |   |   | ' x
    x ' |   |   | ' |
    | ' |   |   | ' |
    | ' x   x   x ' |
    | ' |   |   | ' |
(sw)       (st)      (sw)

The constraints for each node are: z_ee_k x_ee_k - x_ee_k-1 y_ee_k - y_ee_k-1

During swing the bounds are (0,inf) and (-inf,inf) respectively. During stance the bounds are (0,0) for both. Except for the first node of a stance phase! For this node the x-position constraint is also (-inf,inf).

Public Types

using Vector3d = Eigen::Vector3d¶

Public Functions

TerrainFlatConstraint(const RobotModel::Ptr &model, const NodesHolder &nodes_holder, uint ee_id)¶: Constructor

virtual ~TerrainFlatConstraint() = default¶

bool Update(int k) const¶

Update state properties for current node

Return: bool False if this k should be skipped

virtual void UpdateConstraintAtNode(int k, VectorXd &g) const override¶: Get constraint values

virtual void UpdateBoundsAtNode(int k, VecBound &b) const override¶: Get constraint bounds

virtual void UpdateJacobianAtNode(int k, std::string var_set, Jacobian &jac) const override¶: Fill jacobian for constraint

Private Functions

int GetRow(int k, int type) const¶: Get row of constraint vector based on node info

Private Members

uint ee_id_¶

mutable RobotModel::Ptr model_¶: Robot model used for kinematics.

mutable RobotModel::Ptr model_km1_¶

Forces¶

namespace gambol

class ForceConstraint : public gambol::NodesConstraint ¶

#include <ForceConstraint.h>

Constraint for ground reaction forces

This constraint does two things:

Keep force zero during swing phase
Limit force to positive normal and pyramid friction cone

Public Functions

ForceConstraint(const HeightMap::Ptr &terrain, const RobotModel::Ptr &model, const NodesHolder &nodes_holder, double max_normal, uint ee_id)¶: Constructor

virtual ~ForceConstraint() = default¶

virtual void UpdateConstraintAtNode(int k, VectorXd &g) const override¶: Get constraint values

virtual void UpdateBoundsAtNode(int k, VecBound &b) const override¶: Get constraint bounds

virtual void UpdateJacobianAtNode(int k, std::string var_set, Jacobian &jac) const override¶: Fill jacobian for constraint

Private Functions

int GetRow(int k, int type = 0) const¶: Get row of constraint vector based on node info

void Update(int k) const¶: Update state properties for current node

Private Members

HeightMap::Ptr terrain_¶

mutable RobotModel::Ptr model_¶

uint ee_id_¶

double max_normal_force_¶

double mu_¶: Friction coefficient.

namespace gambol

class ForceFlatConstraint : public gambol::NodesConstraint ¶

#include <ForceFlatConstraint.h>

Constraint for ground reaction forces

(This is an older version that does not incorporate terrain)

This constraint does two things:

Keep force zero during swing phase
Limit force to positive normal and pyramid friction cone

Public Functions

ForceFlatConstraint(const NodesHolder &nodes_holder, double max_normal, uint ee_id)¶: Constructor

virtual ~ForceFlatConstraint() = default¶

virtual void UpdateConstraintAtNode(int k, VectorXd &g) const override¶: Get constraint values

virtual void UpdateBoundsAtNode(int k, VecBound &b) const override¶: Get constraint bounds

virtual void UpdateJacobianAtNode(int k, std::string var_set, Jacobian &jac) const override¶: Fill jacobian for constraint

Private Functions

int GetRow(int k, int type) const¶: Get row of constraint vector based on node info

Private Members

uint ee_id_¶

double max_normal_force_¶

double mu_¶: Friction coefficient.

Quaternion¶

namespace gambol

class QuaternionConstraint : public gambol::NodesConstraint ¶

#include <QuaternionConstraint.h>

Constraint quaternions to unity

MuJoCos qpos contains quaternions, which need to be constraint to unity length. Note that internally the quaternions are scaled to 1.0 already, nonetheless it seems wise to constrain the optimisation variables too.

Public Functions

QuaternionConstraint(const RobotModel::Ptr &model, const NodesHolder &nodes_holder)¶

virtual ~QuaternionConstraint() = default¶

Private Functions

int GetRow(int k, int quat = 0) const¶

Return index of constraint based on node info

Get row of constraint vector based on node info

bool Update(int k) const¶: Extract states for current node

virtual void UpdateConstraintAtNode(int k, VectorXd &g) const override¶: Update constraint value at node

virtual void UpdateBoundsAtNode(int k, VecBound &b) const override¶: Set bound for specific node

virtual void UpdateJacobianAtNode(int k, std::string var_set, Jacobian &jac) const override¶

Set jacobian for this node

jac is the jacobian of the entire constraint to the current variable set.

Private Members

RobotModel::Ptr model_¶: Robot model.

NodesVariables::Ptr joint_pos_¶

std::vector<int> quat_ids_¶

int num_quats_¶

Symmetry¶

namespace gambol

class SymmetryConstraint : public ConstraintSet¶

#include <SymmetryConstraint.h>

Constraint to keep the initial pose identical the last pose

Public Types

using VectorXd = Eigen::VectorXd¶

using VecTimes = std::vector<double>¶

using Bounds = ifopt::Bounds¶

Public Functions

SymmetryConstraint(const NodesVariables::Ptr &nodes, const std::vector<int> &dims = {})¶

Constructor

Parameters

nodes: Pointer to NodesVariables to constraint
dims: List of dimensions to constrain (constrain all when left empty)

virtual ~SymmetryConstraint() = default¶

VectorXd GetValues() const override¶: Get constraint values

VecBound GetBounds() const override¶: Get bounds of the constraint

void FillJacobianBlock(std::string var_set, Jacobian &jac) const override¶: Fill in jacobian for the constraint

Private Members

NodesVariables::Ptr nodes_¶

std::vector<int> dims_¶