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

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

Link to optimization variable

virtual double GetCost() const override

Calculate cost value

Protected Functions

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

Calculate section of jacobian

Protected Attributes

NodesVariables::Ptr nodes_
std::string node_id_
int dim_
double weight_
double exp_

Node Derivative Cost

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

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

virtual ~NodeDerivativeCost() = default
virtual double GetCost() const override

Calculate cost value

virtual 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

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

Link variables

virtual double GetCost() const override

Get cost value

Private Functions

bool Update(int k) const

Update the model with the current frame

Return

bool Return false if not using this node

Parameters

  • k:

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

Fill in jacobian

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_