#include <DynamicsOptimizer.hpp>

Collaboration diagram for momentumopt::DynamicsOptimizer:

[legend]

Public Member Functions
	DynamicsOptimizer ()

void	initialize (PlannerSetting &planner_setting)
	function to initialize and configure the optimization More...

solver::ExitCode	optimize (const DynamicsState &ini_state, ContactPlanInterface *contact_plan, const KinematicsSequence &kin_sequence, bool update_tracking_objective=false)
	function to parse equations and objective into optimization problem and attempt to find a solution More...

DynamicsSequence &	dynamicsSequence ()
	this function gives access to the optimized motion plan More...

const DynamicsSequence &	dynamicsSequence () const

const double &	solveTime () const

Private Member Functions
PlannerSetting &	getSetting ()

const PlannerSetting &	getSetting () const

const ContactType	contactType (int time_id, int eff_id) const

const Eigen::Matrix3d	contactRotation (int time_id, int eff_id) const

const double	contactLocation (int time_id, int eff_id, int axis_id) const

void	initializeOptimizationVariables ()
	function to initialize optimization variables: type [continuous or binary], guess value [if any], upper and lower bounds for the variable

void	addVariableToModel (const solver::OptimizationVariable &opt_var, solver::Model &model, std::vector< solver::Var > &vars)
	function to add each variable to the Model and assign a unique identifier to it, to be used by the optimizer to construct the problem More...

void	internalOptimize (const KinematicsSequence &kin_sequence, bool is_first_time=false)
	functions to update tracking objective for momentum from penalty to tracking and attempt to find a solution to the optimization problem More...

void	updateTrackingObjective ()

void	saveSolution (solver::OptimizationVariable &opt_var)
	functions that transfers optimal solution from model to helper class OptimizationVariable, from helper class OptimizationVariable to dynamics sequence to be accessed by the user, and from dynamics sequence to a file. More...

void	storeSolution ()

void	saveToFile (const KinematicsSequence &kin_sequence)

Private Attributes
PlannerSetting *	planner_setting_

solver::Model	model_
	class that stores all information about the problem, interfaces between high level definition of the problem and its corresponding mathematical construction to solve it with a solver instance

solver::LinExpr	lin_cons_

solver::DCPQuadExpr	quad_objective_

solver::DCPQuadExpr	quad_cons_

solver::ExitCode	exitcode_

std::vector< solver::Var >	vars_
	c++ vector containing all problem variables defined by the user. More...

DynamicsState	ini_state_
	initial configuration of the robot, including center of mass position, linear and angular momenta, end-effectors configurations: activation, position, orientation

FrictionCone	friction_cone_

FrictionCone::ConeMat	cone_matrix_

DynamicsSequence	dyn_sequence_
	dynamics sequence of dynamic states. More...

ContactPlanInterface *	contact_plan_
	helper class to store information about the contact plan, such as surfaces, position and orientation of a sequence of contacts

Clock	timer_

char	variable_type_

bool	has_converged_

int	size_

int	num_vars_

double	solve_time_

double	convergence_err_

double	last_convergence_err_

Eigen::Vector3d	com_pos_goal_

Eigen::Vector3d	weight_desired_com_tracking_

solver::OptimizationVariable	dt_

solver::OptimizationVariable	com_

solver::OptimizationVariable	lmom_

solver::OptimizationVariable	amom_

solver::OptimizationVariable	lmomd_

solver::OptimizationVariable	amomd_

std::array< solver::OptimizationVariable, Problem::n_endeffs_ >	frc_world_

std::array< solver::OptimizationVariable, Problem::n_endeffs_ >	trq_local_

std::array< solver::OptimizationVariable, Problem::n_endeffs_ >	cop_local_

std::array< solver::OptimizationVariable, Problem::n_endeffs_ >	ub_var_

std::array< solver::OptimizationVariable, Problem::n_endeffs_ >	lb_var_

solver::OptimizationVariable::OptVector	solution_

solver::OptimizationVariable::OptMatrix	mat_lb_

solver::OptimizationVariable::OptMatrix	mat_ub_

solver::OptimizationVariable::OptMatrix	mat_guess_

solver::OptimizationVariable::OptMatrix	com_guess_

solver::OptimizationVariable::OptMatrix	lmom_guess_

solver::OptimizationVariable::OptMatrix	amom_guess_

Detailed Description

main class of the dynamics optimization problem

Constructor & Destructor Documentation

momentumopt::DynamicsOptimizer::DynamicsOptimizer ( )

inline

default class constructor and destructor

Member Function Documentation

void momentumopt::DynamicsOptimizer::addVariableToModel	(	const solver::OptimizationVariable &	opt_var,
		solver::Model &	model,
		std::vector< solver::Var > &	vars
	)

private

function to add each variable to the Model and assign a unique identifier to it, to be used by the optimizer to construct the problem

Parameters

[in]	opt_var	helper optimization variable for model predictive control
[in]	model	instance of solver interface to collect constraints, objective and solve problem
[in]	vars	vector of optimization variables

const ContactType momentumopt::DynamicsOptimizer::contactType	(	int	time_id,
		int	eff_id
	)		const

private

helper functions for the optimization problem

DynamicsSequence& momentumopt::DynamicsOptimizer::dynamicsSequence ( )

inline

this function gives access to the optimized motion plan

Returns: DynamicsSequence reference to dynamics sequence, which is a collection of dynamic states, each of which contains information about all variables, all end-effectors of the motion plan for one time step

PlannerSetting& momentumopt::DynamicsOptimizer::getSetting ( )

inlineprivate

Getter and setter methods for getting the planner variables

void momentumopt::DynamicsOptimizer::initialize ( PlannerSetting & planner_setting )

function to initialize and configure the optimization

Parameters

[in] PlannerSetting setting of planner configuration variables

void momentumopt::DynamicsOptimizer::internalOptimize	(	const KinematicsSequence &	kin_sequence,
		bool	is_first_time = `false`
	)

private

functions to update tracking objective for momentum from penalty to tracking and attempt to find a solution to the optimization problem

Parameters

[in]	ref_sequence	dynamics sequence to be used as momentum tracking reference (can be zeros).
[in]	is_first_time	flag to indicate if this is the first time the solution is being constructed

ExitCode momentumopt::DynamicsOptimizer::optimize	(	const DynamicsState &	ini_state,
		ContactPlanInterface *	contact_plan,
		const KinematicsSequence &	kin_sequence,
		bool	update_tracking_objective = `false`
	)

function to parse equations and objective into optimization problem and attempt to find a solution

Parameters

[in]	ini_state	initial state of the robot
[in]	contact_plan	container of contact sequence to be used for dynamics planning
[in]	kin_sequence	kinematics sequence to be used as momentum tracking reference (can be zeros).
[in]	update_tracking_objective	changes weights from regulation to tracking of momentum in ref_sequence

Returns: ExitCode flag that indicates the optimization result (for example: optimal, infeasible)

void momentumopt::DynamicsOptimizer::saveSolution ( solver::OptimizationVariable & opt_var )

private

functions that transfers optimal solution from model to helper class OptimizationVariable, from helper class OptimizationVariable to dynamics sequence to be accessed by the user, and from dynamics sequence to a file.

Parameters

[in]	opt_var	helper optimization variable for model predictive control
[in]	ref_sequence	dynamics sequence to be used as momentum tracking reference (can be zeros).

const double& momentumopt::DynamicsOptimizer::solveTime ( ) const

inline

function to have access to time required to solve the optimization problem

Member Data Documentation

Eigen::Vector3d momentumopt::DynamicsOptimizer::com_pos_goal_

private

helper vector variables for the optimization problem

solver::OptimizationVariable momentumopt::DynamicsOptimizer::dt_

private

helper optimization variables for the optimization problem

DynamicsSequence momentumopt::DynamicsOptimizer::dyn_sequence_

private

dynamics sequence of dynamic states.

This is the main interface between the user and the planner. The user can find in this variable all the optimization results

solver::ExitCode momentumopt::DynamicsOptimizer::exitcode_

private

exit code of the optimization problem

FrictionCone momentumopt::DynamicsOptimizer::friction_cone_

private

simple helper class to build a linear approximation of a friction cone

bool momentumopt::DynamicsOptimizer::has_converged_

private

helper boolean variables for the optimization problem

solver::LinExpr momentumopt::DynamicsOptimizer::lin_cons_

private

helper variables to construct linear and quadratic expressions

PlannerSetting* momentumopt::DynamicsOptimizer::planner_setting_

private

Variables required for optimization

int momentumopt::DynamicsOptimizer::size_

private

helper integer variables for the optimization problem

solver::OptimizationVariable::OptVector momentumopt::DynamicsOptimizer::solution_

private

helper matrices and vectors for the optimization problem

double momentumopt::DynamicsOptimizer::solve_time_

private

helper double variables for the optimization problem

Clock momentumopt::DynamicsOptimizer::timer_

private

clock for timing purposes

char momentumopt::DynamicsOptimizer::variable_type_

private

type for optimization variable: continuous 'C' or binary 'B' and type of heuristic

std::vector<solver::Var> momentumopt::DynamicsOptimizer::vars_

private

c++ vector containing all problem variables defined by the user.

Does not include extra variables required to write the problem in standard conic form.

The documentation for this class was generated from the following files:

include/momentumopt/dynopt/DynamicsOptimizer.hpp
src/momentumopt/dynopt/DynamicsOptimizer.cpp

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation