Control

The following classes are used to realise the generated motion on a simulated system.

MPC

namespace gambol
class MPC
#include <MPC.h>

Wrapper for model predictive control for simulation

Functionality is fairly basic, main purpose is to wrap the optimization update.

Public Types

using VectorXd = Eigen::VectorXd

Public Functions

MPC()

Constructor

virtual ~MPC() = default
void SetInitialConfig(const VectorXd &qpos, const VectorXd &qvel)

Set new initial configuration

This is used to fix the MPC to the current state.

void SetTime(double t)

Update time

Bring phases closer by shortening the tip. End of phases is padded with contact (stance mode) to achieve a rolling horizion.

bool Predict(NodesHolder &solution)

Perform optimization on current configurations

Function is blocking until optimization is done.

Return

True if solver return is SUCCESS or ACCEPTABLE

Parameters

Public Members

NlpFormulation formulation_
ifopt::IpoptSolver::Ptr solver_
ifopt::Problem nlp_
int optimizations_

Private Functions

void InterpolateOldSolution(ifopt::VariableSet::Ptr set, const NodesHolder &old_solution)

Interpolate a new variable set from a previous solution

NodesVariables::Ptr GetVariableFromHolder(const NodesHolder &holder, const std::string &name) const

Get variable from NodesHolder based on name

Private Members

double t_last_

Simulation time of last update.

double delta_t_

Simulation time difference between last updates.

Robot State

namespace gambol
struct RobotState
#include <RobotState.h>

Simple struct which completely defines a robot in a frame

Public Types

using VectorXd = Eigen::VectorXd

Public Functions

RobotState() = default
RobotState(const VectorXd &_qpos, const VectorXd &_qvel, const VectorXd &_u_ff, const VectorXd &_u_fb)

Constructor

virtual ~RobotState() = default

Public Members

VectorXd qpos
VectorXd qvel
VectorXd u_ff
VectorXd u_fb

CsvLogger

namespace gambol
class CsvLogger
#include <CsvLogger.h>

Class to log simulation data to file

It is made to log during an online MPC scheme, so it logs both optimized trajectories and simulated data when this trajectory is applied.

Public Types

using VectorXd = Eigen::VectorXd

Public Functions

CsvLogger(std::string name = "")

Constructor

Parameters

  • name: Name for part of the file, e.g. model name

virtual ~CsvLogger() = default

Filestream is RAII and closes itself

void log(double t, const VectorXd &qpos, const VectorXd &qvel, const VectorXd &qacc, const VectorXd &qpos_r, const VectorXd &qvel_r, const VectorXd &u_ff, const VectorXd &u_fb, const std::vector<VectorXd> &forces, const std::vector<VectorXd> &forces_r, int predict_count = 0)

Create new log entry

Create log entry

Public Static Functions

static std::string fileNameFromPath(std::string path)

Return filename from path

Return file name from a path

Private Members

std::ofstream log_file_
int rows_