#include <KinematicsOptimizer.hpp>

Collaboration diagram for momentumopt::KinematicsOptimizer:

[legend]

Public Member Functions
	KinematicsOptimizer ()

void	initialize (PlannerSetting &planner_setting, KinematicsInterface *kin_interface)
	function to initialize and configure the optimization More...

void	optimize (const DynamicsState &ini_state, ContactPlanInterface *contact_plan, DynamicsSequence &dyn_sequence, bool is_first_kinopt)
	function to parse equations and objective into optimization problem and attempt to find a solution More...

KinematicsSequence &	kinematicsSequence ()
	this function gives access to the optimized motion plan More...

const KinematicsSequence &	kinematicsSequence () const

Private Member Functions
PlannerSetting &	getSetting ()
	helper function to replay a motion More...

const PlannerSetting &	getSetting () const

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

void	initializeTrajectoryKinematicVariables ()

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	saveSolution (solver::OptimizationVariable &opt_var)
	functions that transfers optimal solution from model to helper class OptimizationVariable, and from helper class OptimizationVariable to dynamics sequence to be accessed by the user More...

void	storeSolution (const DynamicsState &ini_state, DynamicsSequence &dyn_sequence)

void	optimizeTrajectory (const DynamicsState &ini_state, DynamicsSequence &dyn_sequence)
	function that implements iterative inverse kinematics to track momentum, end-effector motions, by a robot with a given joint configuration More...

void	optimizePosture (KinematicsState &current_state, const DynamicsState &desired_state, bool is_initial_state=false)

void	displayMotion (const DynamicsSequence &dyn_sequence)

void	updateEndeffectorTrajectories (DynamicsSequence &dyn_sequence)
	function to update the tracking endeffector trajectories More...

Eigen::Vector3d	quaternionToRotationVector (const Eigen::Quaternion< double > &q) const

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::ExitCode	exitcode_

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

KinematicsState *	ini_state_
	configuration of the robot, including center of mass position, linear and angular momenta, end-effectors configurations: activation, position, orientation, joint configuration: positions, velocities and accelerations, for initial state, current state and desired state

KinematicsState *	current_state_

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

KinematicsSequence	kin_sequence_
	kinematics sequence of kinematics states. More...

KinematicsInterface *	kin_interface_

char	variable_type_

int	size_

int	num_vars_

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

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

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

solver::OptimizationVariable	jnt_q_

solver::OptimizationVariable	jnt_qd_

solver::OptimizationVariable	jnt_qdd_

solver::OptimizationVariable	total_qd_

solver::OptimizationVariable	total_qdd_

solver::OptimizationVariable	com_

solver::OptimizationVariable	comd_

solver::OptimizationVariable	lmom_

solver::OptimizationVariable	amom_

solver::OptimizationVariable	base_position_

solver::OptimizationVariable	base_ang_vel_

solver::OptimizationVariable	slack_vars_

solver::OptimizationVariable::OptVector	solution_

solver::OptimizationVariable::OptMatrix	mat_guess_

solver::OptimizationVariable::OptMatrix	mat_lb_

solver::OptimizationVariable::OptMatrix	mat_ub_

std::array< Eigen::Vector3d, Problem::n_endeffs_ >	desired_eff_velocity_

Eigen::Vector3d	desired_com_velocity_

Eigen::Vector3d	desired_lmom_velocity_

Eigen::Vector3d	desired_amom_velocity_

Detailed Description

main class of the kinematic optimization problem

Constructor & Destructor Documentation

momentumopt::KinematicsOptimizer::KinematicsOptimizer ( )

inline

default class constructor and destructor

Member Function Documentation

void momentumopt::KinematicsOptimizer::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

void momentumopt::KinematicsOptimizer::displayMotion ( const DynamicsSequence & dyn_sequence )

private

helper function to display a kinematic motion

PlannerSetting& momentumopt::KinematicsOptimizer::getSetting ( )

inlineprivate

helper function to replay a motion

Parameters

[in]	PlannerSetting	setting of planner configuration variables
[in]	kin_interface	implementation of robot specific functions needed by the optimizer
[in]	load_file	file where the kinematic parameters have been stored function to load a kinematic solution from a file and fill a sequence with it
[in]	load_file	name of configuration file from where to load the solution

Getter and setter methods for getting the planner variables

void momentumopt::KinematicsOptimizer::initialize	(	PlannerSetting &	planner_setting,
		KinematicsInterface *	kin_interface
	)

function to initialize and configure the optimization

Parameters

[in]	PlannerSetting	setting of planner configuration variables
[in]	kin_interface	implementation of robot specific functions needed by the optimizer

KinematicsSequence& momentumopt::KinematicsOptimizer::kinematicsSequence ( )

inline

this function gives access to the optimized motion plan

Returns: DynamicsSequence reference to kinematic sequence, which is a collection of kinematic states, each of which contains information about all variables, all end-effectors of the motion plan, joint positions, velocities and accelerations for one time step

void momentumopt::KinematicsOptimizer::optimize	(	const DynamicsState &	ini_state,
		ContactPlanInterface *	contact_plan,
		DynamicsSequence &	dyn_sequence,
		bool	is_first_kinopt
	)

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]	dyn_sequence	dynamics sequence to be used as momentum tracking reference

void momentumopt::KinematicsOptimizer::optimizeTrajectory	(	const DynamicsState &	ini_state,
		DynamicsSequence &	dyn_sequence
	)

private

function that implements iterative inverse kinematics to track momentum, end-effector motions, by a robot with a given joint configuration

Parameters

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

private

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

Parameters

[in] opt_var helper optimization variable for model predictive control

void momentumopt::KinematicsOptimizer::updateEndeffectorTrajectories ( DynamicsSequence & dyn_sequence )

private

function to update the tracking endeffector trajectories

Parameters

[in] dyn_sequence dynamics sequence to be used as momentum tracking reference

Member Data Documentation

std::array<solver::OptimizationVariable, Problem::n_endeffs_> momentumopt::KinematicsOptimizer::eef_pos_

private

helper optimization variables for the optimization problem

solver::ExitCode momentumopt::KinematicsOptimizer::exitcode_

private

exit code of the optimization problem

KinematicsInterface* momentumopt::KinematicsOptimizer::kin_interface_

private

implementation of robot specific functions needed by the optimizer

KinematicsSequence momentumopt::KinematicsOptimizer::kin_sequence_

private

kinematics sequence of kinematics states.

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

solver::LinExpr momentumopt::KinematicsOptimizer::lin_cons_

private

helper variables to construct linear and quadratic expressions

PlannerSetting* momentumopt::KinematicsOptimizer::planner_setting_

private

Variables required for optimization

int momentumopt::KinematicsOptimizer::size_

private

helper integer variables for the optimization problem

solver::OptimizationVariable::OptVector momentumopt::KinematicsOptimizer::solution_

private

helper matrices and vectors for the optimization problem

char momentumopt::KinematicsOptimizer::variable_type_

private

type for optimization variable: continuous 'C' or binary 'B'

std::vector<solver::Var> momentumopt::KinematicsOptimizer::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/kinopt/KinematicsOptimizer.hpp
src/momentumopt/kinopt/KinematicsOptimizer.cpp

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation