Consts

Costs in the NLP are created through the following classes.

Node Cost

namespace gambol
class NodeCost : public CostTerm
#include <NodeCost.h>

Cost term based on a node value

Cost is the squared sum of node value. One instance of this class governs a single dimension.

Subclassed by gambol::NodeDerivativeCost

Public Functions

NodeCost(const std::string &nodes_id, int dim, double weight, double exp = 2.0, const std::string &prefix = "")

Constructor.

Parameters

  • nodes_id – Name of the linked variable

  • dim – Dimension of this variable to use

  • weight – Relative weight of this cost

  • exp – 2 by default (square)

  • prefix – Extra prefix to the name of this object

~NodeCost() override = default
void InitVariableDependedQuantities(const VariablesPtr &x) override

Link to optimization variable

double GetCost() const override

Calculate cost value

Protected Functions

void FillJacobianBlock(std::string var_set, Jacobian&) const override

Calculate section of jacobian

Protected Attributes

NodesVariables::Ptr nodes_

Pointer to variable to use as cost.

std::string node_id_

Name of the variable.

int dim_

Dimension of this variable to use.

double weight_
double exp_

Node Derivative Cost

namespace gambol
class NodeDerivativeCost : public gambol::NodeCost
#include <NodeDerivativeCost.h>

Cost based on the derivative of a NodesVariable.

The derivative is calculated as the scaled difference between adjacent nodes. So the final sum will have N-1 terms.

Public Functions

NodeDerivativeCost(const std::string &nodes_id, int dim, double weight)

Constructor.

Parameters

  • nodes_id – Name of the linked variable

  • dim – Dimension of this variable to use

  • weight – Relative weight of this cost

~NodeDerivativeCost() override = default

Destructor.

double GetCost() const override

Calculate cost value

void FillJacobianBlock(std::string var_set, Jacobian &jac) const override

Calculate section of jacobian

Foot Lift Reward

namespace gambol
class FootLiftReward : public CostTerm
#include <FootLiftReward.h>

Reward (negative cost) for the height of a foot above the ground.

Public Types

using Vector3d = Eigen::Vector3d

Public Functions

FootLiftReward(const NodeTimes::Ptr &node_times, const std::vector<PhaseDurations::Ptr> &phase_durations, const RobotModel::Ptr &model, uint ee_id, double weight)

Constructor.

Parameters

  • node_times –

  • phase_durations –

  • model –

  • ee_id – End-effector this applies to

  • weight – Weight should be positive to make reward

~FootLiftReward() override = default

Destructor.

void InitVariableDependedQuantities(const VariablesPtr &x) override

Link variables.

double GetCost() const override

Get cost value.

Private Functions

bool Update(int k) const

Update the model with the current frame.

Parameters

k –

Returns

bool Return false if not using this node

void FillJacobianBlock(std::string var_set, Jacobian&) const override

Fill in jacobian section.

Private Members

mutable RobotModel::Ptr model_
NodeTimes::Ptr node_times_
NodesVariables::Ptr joint_pos_
std::vector<PhaseDurations::Ptr> phase_durations_
uint ee_id_
double weight_