Variables

Variables in the NLP are created through the following classes.

Nodes Variables

namespace gambol

class NodesVariables : public VariableSet

#include <NodesVariables.h>

Variables that occur on each collocation node.

It is assumed basic interpolation is used between nodes. No splines are included in the variable or their optimization.

Public Types

using Ptr = std::shared_ptr<NodesVariables>

using VectorXd = Eigen::VectorXd

Public Functions

NodesVariables(int n_nodes, int n_dim, const std::string &variable_id, const NodeTimes::Ptr &node_times)

Constructor

~NodesVariables() override = default

virtual NodeValueInfo GetNodeInfo(int opt_idx) const

Get info of node affected by one specific optimization variable.

Reverse of GetOptIndex().

Parameters

opt_idx – The index (=row) of the optimization variable.

Returns

The node value affected by this optimization variable.

virtual int GetOptIndex(const NodeValueInfo &nvi_des) const

Get index of specific value inside the big stacked opt. vector

This function simply overloads the basic and doesn’t require a NVI struct.

virtual int GetOptIndex(int node_id, int dim) const

Get opt index of specific value, without the NVI struct

VectorXd GetValues() const override

Pure optimization variables that define the nodes

void SetVariables(const VectorXd &x) override

Sets node positions from the optimization variables.

Parameters

x – The optimization variables.

void SetVariables(const std::vector<VectorXd> &nodes)

Set nodes directly

int GetDim() const

Return number of dimensions

int GetNumNodes() const

Return number of nodes

VecBound GetBounds() const override

Returns

the bounds on position and velocity of each node and dimension.

const std::vector<VectorXd> &GetNodes() const

Returns

All the nodes.

const VectorXd &GetNode(int k) const

Returns

A single node.

VectorXd GetPoint(double t) const

Returns

An interpolated point by time

void SetNode(int k, const VectorXd &value)

Set specific node value

Jacobian GetNodeJacobian(int k) const

Return jacobian of a node w.r.t. the entire variable stack.

Resulting jacobian has dim rows and dim * n_nodes columns. It will consist of zeros with an identity block.

Parameters

k – ID of the node to which the jacobian is for

void SetByLinearInterpolation(const VectorXd &initial_val, const VectorXd &final_val)

Sets nodes pos/vel equally spaced from initial to final position.

Parameters

• initial_val – value of the first node.

• final_val – value of the final node.

void SetConstantByPhase(const VectorXd &value, bool contact, const PhaseDurations::Ptr &phases)

Sets node values for contact or flight phase.

Parameters

• value – Constant value to be set

• contact – Which phase to set

• phases – Pointer to phases definition

void AddStartBound(const std::vector<int> &dimensions, const VectorXd &val)

Restricts the first node in the spline.

Parameters

• dimensions – Which dimensions (x,y,z) should be restricted.

• val – The values the fist node should be set to.

void AddFinalBound(const std::vector<int> &dimensions, const VectorXd &val)

Restricts the last node in the spline.

Parameters

• dimensions – Which dimensions (x,y,z) should be restricted.

• val – The values the last node should be set to.

void AddGlobalBound(const std::vector<int> &dimensions, const std::vector<std::pair<double, double>> &limits)

Set constant limits to all joints

This will overwrite Start and Final bounds!

Parameters

• dimensions – List of dimensions to restrict

• limits – The min and max applied to each node

Protected Attributes

VecBound bounds_

The bounds on the node values.

std::vector<VectorXd> nodes_

Actual nodes.

int n_dim_

Number of values (=dimensions_ per node.

NodeTimes::Ptr node_times_

Pointer to node timings.

Private Functions

void AddBounds(int node_id, const std::vector<int> &dim, const VectorXd &values)

Bounds a specific node variables.

Parameters

• node_id – The ID of the node to bound.

• dim – The dimension of the node to bound.

• values – The values to set the bounds to.

void AddBound(const NodeValueInfo &node_info, double value)

Restricts a specific optimization variables.

Parameters

• node_info – The specs of the optimization variables to restrict.

• value – The value to set the bounds to.

struct NodeValueInfo

#include <NodesVariables.h>

Semantic information associated with a scalar node value

This includes all information except the actual value. This comes from the vector of optimization variables.

Public Functions

NodeValueInfo() = default

Default constructor

NodeValueInfo(int node_id, int node_dim)

Useful constructor

Parameters

• node_id –

• node_dim –

int operator==(const NodeValueInfo &right) const

Overloaded equality-check operator. Return true when both id and dim are equal.

Parameters

right –

Returns

Public Members

int id_

ID of the associated node (0 =< id < number of nodes in spline).

int dim_

Dimension (x,y,z or joint id) of that derivative.

Node Times

namespace gambol

class NodeTimes

#include <NodeTimes.h>

Class for the times of each node (= collocation point)

This class is not essential, but it makes it easier to share a single instance of nodes times between different components.

Note: NodeTimes are not actually optimized over, so they are not optimization variables.

Public Types

using Ptr = std::shared_ptr<NodeTimes>

using VecTimes = std::vector<double>

Public Functions

explicit NodeTimes(const VecTimes &times)

Construct with entire list

Parameters

times – List of all node times

NodeTimes(double t_total, int n_nodes)

Construct with delta_t and number of nodes

Parameters

• t_total – Total duration

• n_nodes – Number of nodes

virtual ~NodeTimes() = default

Destructor

int GetNumberOfNodes() const

Return number of nodes

double GetTotalTime() const

Get total duration

double at(int k) const

Get time of node k

const VecTimes &GetList() const

Get entire list

int GetNodeId(double t_des) const

Return id of node right before given time.

Return first node when t < 0 and last node when t > GetTotalTime().

int GetNodeId(double t_des, double &interpolation) const

Return id of node right before given time and the interpolation factor

Most right node is ignored. Method helps to interpolate random objects.

double GetDeltaT(int k) const

Get duration of this node and the next

Throws error when index is out of range

Private Members

VecTimes node_times_

Phase Durations

namespace gambol

class PhaseDurations

#include <PhaseDurations.h>

Class for durations of contact phases.

This class determined whether a node is part of stance or swing. These durations are not optimized over, so they are not optimization variables.

Public Types

using Ptr = std::shared_ptr<PhaseDurations>

using VecDurations = std::vector<double>

Public Functions

PhaseDurations(const VecDurations &initial_durations, bool is_first_phase_in_contact)

Constructor.

Parameters

• timings – List of phase timings

• is_first_phase_in_contact – true to start in contact

virtual ~PhaseDurations() = default

Destructor.

const VecDurations &GetPhaseDurations() const

Return vector with all durations.

bool IsContactPhase(double t) const

Check whether in contact at a certain time

Private Members

VecDurations durations_

Actual durations.

double t_total_

bool initial_contact_state_

True if first phase in contact.

Variable Names

namespace gambol

namespace id

Provide constants to recognize the variable names

Variables

static const std::string joint_pos = "joint-pos"

static const std::string joint_vel = "joint-vel"

static const std::string torques = "torques"

static const std::string forces = "ee-forces_"

Nodes Holder

namespace gambol

struct NodesHolder

#include <NodesHolder.h>

Struct to group all pointers to important variables together.

Public Functions

NodesHolder() = default

Default constructor

NodesHolder(const NodesVariables::Ptr &joint_pos, const NodesVariables::Ptr &joint_vel, const NodesVariables::Ptr &torques, const std::vector<NodesVariables::Ptr> &ee_forces, const std::vector<PhaseDurations::Ptr> &phase_durations, const NodeTimes::Ptr &nodes_times)

Real constructor

virtual ~NodesHolder() = default

Destructor

Public Members

NodesVariables::Ptr joint_pos_

NodesVariables::Ptr joint_vel_

NodesVariables::Ptr torques_

std::vector<NodesVariables::Ptr> ee_forces_

std::vector<PhaseDurations::Ptr> phase_durations_

NodeTimes::Ptr node_times_

Coordinates

Enums

enum Dim2D

Indicate dimension in 2D.

Values:

enumerator X_

enumerator Y_

enum Dim3D

Indicate dimension in 3D.

Values:

enumerator X

enumerator Y

enumerator Z