Bound constrained time-indexed problem. More...

#include <bounded_time_indexed_problem.h>

Inheritance diagram for exotica::BoundedTimeIndexedProblem:

Collaboration diagram for exotica::BoundedTimeIndexedProblem:

Public Member Functions
	BoundedTimeIndexedProblem ()=default

virtual	~BoundedTimeIndexedProblem ()=default

void	Instantiate (const BoundedTimeIndexedProblemInitializer &init) override
	Instantiates the problem from an Initializer. More...

void	PreUpdate () override
	Updates internal variables before solving, e.g., after setting new values for Rho. More...

void	Update (Eigen::VectorXdRefConst x, int t) override
	Updates an individual timestep from a given state vector. More...

bool	IsValid () override
	Evaluates whether the problem is valid. More...

void	SetGoalEQ (const std::string &task_name, Eigen::VectorXdRefConst goal, int t=0)=delete

Eigen::VectorXd	GetGoalEQ (const std::string &task_name, int t=0)=delete

void	SetRhoEQ (const std::string &task_name, const double rho, int t=0)=delete

double	GetRhoEQ (const std::string &task_name, int t=0)=delete

void	SetGoalNEQ (const std::string &task_name, Eigen::VectorXdRefConst goal, int t=0)=delete

Eigen::VectorXd	GetGoalNEQ (const std::string &task_name, int t=0)=delete

void	SetRhoNEQ (const std::string &task_name, const double rho, int t=0)=delete

double	GetRhoNEQ (const std::string &task_name, int t=0)=delete

Eigen::VectorXd	GetEquality () const =delete

Eigen::VectorXd	GetInequality () const =delete

Eigen::SparseMatrix< double >	GetEqualityJacobian () const =delete

Eigen::SparseMatrix< double >	GetInequalityJacobian () const =delete

std::vector< Eigen::Triplet< double > >	GetEqualityJacobianTriplets () const =delete

int	get_active_nonlinear_equality_constraints_dimension () const =delete

Eigen::VectorXd	GetEquality (int t) const =delete

Eigen::MatrixXd	GetEqualityJacobian (int t) const =delete

Eigen::VectorXd	GetInequality (int t) const =delete

Eigen::MatrixXd	GetInequalityJacobian (int t) const =delete

std::vector< Eigen::Triplet< double > >	GetInequalityJacobianTriplets () const =delete

int	get_active_nonlinear_inequality_constraints_dimension () const =delete

int	get_joint_velocity_constraint_dimension () const =delete

Eigen::VectorXd	GetJointVelocityConstraint () const =delete

Eigen::MatrixXd	GetJointVelocityConstraintBounds () const =delete

std::vector< Eigen::Triplet< double > >	GetJointVelocityConstraintJacobianTriplets () const =delete

Eigen::VectorXd	GetJointVelocityLimits () const =delete

void	SetJointVelocityLimits (const Eigen::VectorXd &qdot_max_in)=delete

void	Update (Eigen::VectorXdRefConst x_trajectory_in)
	Updates the entire problem from a given trajectory (e.g., used in an optimization solver) More...

virtual void	Update (Eigen::VectorXdRefConst x_in, int t)
	Updates an individual timestep from a given state vector. More...

Public Member Functions inherited from exotica::AbstractTimeIndexedProblem
	AbstractTimeIndexedProblem ()

virtual	~AbstractTimeIndexedProblem ()

void	Update (Eigen::VectorXdRefConst x_trajectory_in)
	Updates the entire problem from a given trajectory (e.g., used in an optimization solver) More...

double	GetDuration () const
	Returns the duration of the trajectory (T * tau). More...

std::vector< Eigen::VectorXd >	GetInitialTrajectory () const
	Returns the initial trajectory/seed. More...

void	SetInitialTrajectory (const std::vector< Eigen::VectorXd > &q_init_in)
	Sets the initial trajectory/seed. More...

void	SetGoal (const std::string &task_name, Eigen::VectorXdRefConst goal, int t=0)
	Sets goal for a given task at a given timestep (cost task). More...

Eigen::VectorXd	GetGoal (const std::string &task_name, int t=0)
	Returns the goal for a given task at a given timestep (cost task). More...

void	SetRho (const std::string &task_name, const double rho, int t=0)
	Sets Rho for a given task at a given timestep (cost task). More...

double	GetRho (const std::string &task_name, int t=0)
	Returns the precision (Rho) for a given task at a given timestep (cost task). More...

void	SetGoalEQ (const std::string &task_name, Eigen::VectorXdRefConst goal, int t=0)
	Sets goal for a given task at a given timestep (equality task). More...

Eigen::VectorXd	GetGoalEQ (const std::string &task_name, int t=0)
	Returns the goal for a given task at a given timestep (equality task). More...

void	SetRhoEQ (const std::string &task_name, const double rho, int t=0)
	Sets Rho for a given task at a given timestep (equality task). More...

double	GetRhoEQ (const std::string &task_name, int t=0)
	Returns the precision (Rho) for a given task at a given timestep (equality task). More...

void	SetGoalNEQ (const std::string &task_name, Eigen::VectorXdRefConst goal, int t=0)
	Sets goal for a given task at a given timestep (inequality task). More...

Eigen::VectorXd	GetGoalNEQ (const std::string &task_name, int t=0)
	Returns the goal for a given task at a given timestep (goal task). More...

void	SetRhoNEQ (const std::string &task_name, const double rho, int t=0)
	Sets Rho for a given task at a given timestep (inequality task). More...

double	GetRhoNEQ (const std::string &task_name, int t=0)
	Returns the precision (Rho) for a given task at a given timestep (equality task). More...

Eigen::MatrixXd	GetBounds () const
	Returns the joint bounds (first column lower, second column upper). More...

int	GetT () const
	Returns the number of timesteps in the trajectory. More...

void	SetT (const int T_in)
	Sets the number of timesteps in the trajectory. Note: Rho/Goal need to be updated for every timestep after calling this method. More...

double	GetTau () const
	Returns the time discretization tau for the trajectory. More...

void	SetTau (const double tau_in)
	Sets the time discretization tau for the trajectory. More...

double	get_ct () const
	Returns the cost scaling factor. More...

double	GetScalarTaskCost (int t) const
	Returns the scalar task cost at timestep t. More...

Eigen::RowVectorXd	GetScalarTaskJacobian (int t) const
	Returns the Jacobian of the scalar task cost at timestep t. More...

double	GetScalarTransitionCost (int t) const
	Returns the scalar transition cost (x^TWx) at timestep t. More...

Eigen::RowVectorXd	GetScalarTransitionJacobian (int t) const
	Returns the Jacobian of the transition cost at timestep t. More...

double	GetCost () const
	Returns the scalar cost for the entire trajectory (both task and transition cost). More...

Eigen::RowVectorXd	GetCostJacobian () const
	Returns the Jacobian of the scalar cost over the entire trajectory (Jacobian of GetCost). More...

Eigen::VectorXd	GetEquality () const
	Returns the equality constraint values for the entire trajectory. More...

Eigen::VectorXd	GetInequality () const
	Returns the inequality constraint values for the entire trajectory. More...

Eigen::SparseMatrix< double >	GetEqualityJacobian () const
	Returns the sparse Jacobian matrix of the equality constraints over the entire trajectory. More...

Eigen::SparseMatrix< double >	GetInequalityJacobian () const
	Returns the sparse Jacobian matrix of the inequality constraints over the entire trajectory. More...

std::vector< Eigen::Triplet< double > >	GetEqualityJacobianTriplets () const
	Returns a vector of triplets to fill a sparse Jacobian for the equality constraints. More...

int	get_active_nonlinear_equality_constraints_dimension () const
	Returns the dimension of the active equality constraints. More...

Eigen::VectorXd	GetEquality (int t) const
	Returns the value of the equality constraints at timestep t. More...

Eigen::MatrixXd	GetEqualityJacobian (int t) const
	Returns the Jacobian of the equality constraints at timestep t. More...

Eigen::VectorXd	GetInequality (int t) const
	Returns the value of the inequality constraints at timestep t. More...

Eigen::MatrixXd	GetInequalityJacobian (int t) const
	Returns the Jacobian of the inequality constraints at timestep t. More...

std::vector< Eigen::Triplet< double > >	GetInequalityJacobianTriplets () const
	Returns a vector of triplets to fill a sparse Jacobian for the inequality constraints. More...

int	get_active_nonlinear_inequality_constraints_dimension () const
	Returns the dimension of the active inequality constraints. More...

int	get_joint_velocity_constraint_dimension () const
	Returns the dimension of the joint velocity constraint (linear inequality). More...

Eigen::VectorXd	GetJointVelocityConstraint () const
	Returns the joint velocity constraint inequality terms (linear). More...

Eigen::MatrixXd	GetJointVelocityConstraintBounds () const
	Returns the joint velocity constraint bounds (constant terms). More...

std::vector< Eigen::Triplet< double > >	GetJointVelocityConstraintJacobianTriplets () const
	Returns the joint velocity constraint Jacobian as triplets. More...

Eigen::VectorXd	GetJointVelocityLimits () const
	Returns the per-DoF joint velocity limit vector. More...

void	SetJointVelocityLimits (const Eigen::VectorXd &qdot_max_in)
	Sets the joint velocity limits. Supports N- and 1-dimensional vectors. More...

Public Member Functions inherited from exotica::PlanningProblem
	PlanningProblem ()

virtual	~PlanningProblem ()

void	InstantiateBase (const Initializer &init) override

TaskMapMap &	GetTaskMaps ()

TaskMapVec &	GetTasks ()

ScenePtr	GetScene () const

std::string	Print (const std::string &prepend) const override

void	SetStartState (Eigen::VectorXdRefConst x)

Eigen::VectorXd	GetStartState () const

virtual Eigen::VectorXd	ApplyStartState (bool update_traj=true)

void	SetStartTime (double t)

double	GetStartTime () const

unsigned int	GetNumberOfProblemUpdates () const

void	ResetNumberOfProblemUpdates ()

std::pair< std::vector< double >, std::vector< double > >	GetCostEvolution () const

int	GetNumberOfIterations () const

double	GetCostEvolution (int index) const

void	ResetCostEvolution (size_t size)

void	SetCostEvolution (int index, double value)

KinematicRequestFlags	GetFlags () const

int	get_num_positions () const
	! Dimension of planning problem. TODO: Update from positions/velocities/controls and make private. More...

int	get_num_velocities () const

int	get_num_controls () const

Public Member Functions inherited from exotica::Object
	Object ()

virtual	~Object ()

virtual std::string	type () const
	Type Information wrapper: must be virtual so that it is polymorphic... More...

std::string	GetObjectName ()

void	InstantiateObject (const Initializer &init)

Public Member Functions inherited from exotica::InstantiableBase
	InstantiableBase ()=default

virtual	~InstantiableBase ()=default

virtual std::vector< Initializer >	GetAllTemplates () const =0

Public Member Functions inherited from exotica::Instantiable< BoundedTimeIndexedProblemInitializer >
void	InstantiateInternal (const Initializer &init) override

Initializer	GetInitializerTemplate () override

std::vector< Initializer >	GetAllTemplates () const override

const BoundedTimeIndexedProblemInitializer &	GetParameters () const

Private Member Functions
void	ReinitializeVariables () override

Additional Inherited Members
Public Attributes inherited from exotica::AbstractTimeIndexedProblem
TimeIndexedTask	cost
	Cost task. More...

TimeIndexedTask	inequality
	General inequality task. More...

TimeIndexedTask	equality
	General equality task. More...

Eigen::MatrixXd	W

std::vector< TaskSpaceVector >	Phi

std::vector< Eigen::MatrixXd >	jacobian

std::vector< Hessian >	hessian

int	length_Phi

int	length_jacobian

int	num_tasks

bool	use_bounds

Public Attributes inherited from exotica::PlanningProblem
double	t_start

TerminationCriterion	termination_criterion

int	N = 0

Public Attributes inherited from exotica::Object
std::string	ns_

std::string	object_name_

bool	debug_

Protected Member Functions inherited from exotica::AbstractTimeIndexedProblem
void	ValidateTimeIndex (int &t_in) const
	Checks the desired time index for bounds and supports -1 indexing. More...

Protected Member Functions inherited from exotica::PlanningProblem
void	UpdateTaskKinematics (std::shared_ptr< KinematicResponse > response)

void	UpdateMultipleTaskKinematics (std::vector< std::shared_ptr< KinematicResponse >> responses)

Protected Attributes inherited from exotica::AbstractTimeIndexedProblem
int	T_ = 0
	Number of time steps. More...

double	tau_ = 0
	Time step duration. More...

std::vector< Eigen::VectorXd >	x
	Current internal problem state. More...

std::vector< Eigen::VectorXd >	xdiff

double	w_scale_ = 1.0
	Kinematic system transition error covariance multiplier (constant throughout the trajectory) More...

TaskSpaceVector	cost_Phi

TaskSpaceVector	inequality_Phi

TaskSpaceVector	equality_Phi

std::vector< Eigen::VectorXd >	initial_trajectory_

std::vector< std::shared_ptr< KinematicResponse > >	kinematic_solutions_

double	ct
	Normalisation of scalar cost and Jacobian over trajectory length. More...

Eigen::VectorXd	q_dot_max_
	Joint velocity limit (rad/s) More...

Eigen::VectorXd	xdiff_max_
	Maximum change in the variables in a single timestep tau_. Gets set/updated via SetJointVelocityLimits or ReinitializeVariables. More...

std::vector< std::pair< int, int > >	active_nonlinear_equality_constraints_

std::vector< std::pair< int, int > >	active_nonlinear_inequality_constraints_

int	active_nonlinear_equality_constraints_dimension_ = 0

int	active_nonlinear_inequality_constraints_dimension_ = 0

int	joint_velocity_constraint_dimension_ = 0

std::vector< Eigen::Triplet< double > >	joint_velocity_constraint_jacobian_triplets_

Protected Attributes inherited from exotica::PlanningProblem
ScenePtr	scene_

TaskMapMap	task_maps_

TaskMapVec	tasks_

KinematicRequestFlags	flags_ = KinematicRequestFlags::KIN_FK

Eigen::VectorXd	start_state_

unsigned int	number_of_problem_updates_ = 0

std::vector< std::pair< std::chrono::high_resolution_clock::time_point, double > >	cost_evolution_

Protected Attributes inherited from exotica::Instantiable< BoundedTimeIndexedProblemInitializer >
BoundedTimeIndexedProblemInitializer	parameters_

Detailed Description

Bound constrained time-indexed problem.

Constructor & Destructor Documentation

◆ BoundedTimeIndexedProblem()

exotica::BoundedTimeIndexedProblem::BoundedTimeIndexedProblem ( )

default

◆ ~BoundedTimeIndexedProblem()

virtual exotica::BoundedTimeIndexedProblem::~BoundedTimeIndexedProblem ( )

virtualdefault

Member Function Documentation

◆ get_active_nonlinear_equality_constraints_dimension()

int exotica::BoundedTimeIndexedProblem::get_active_nonlinear_equality_constraints_dimension ( ) const

delete

◆ get_active_nonlinear_inequality_constraints_dimension()

int exotica::BoundedTimeIndexedProblem::get_active_nonlinear_inequality_constraints_dimension ( ) const

delete

◆ get_joint_velocity_constraint_dimension()

int exotica::BoundedTimeIndexedProblem::get_joint_velocity_constraint_dimension ( ) const

delete

◆ GetEquality() [1/2]

Eigen::VectorXd exotica::BoundedTimeIndexedProblem::GetEquality ( ) const

delete

◆ GetEquality() [2/2]

Eigen::VectorXd exotica::BoundedTimeIndexedProblem::GetEquality ( int t ) const

delete

◆ GetEqualityJacobian() [1/2]

Eigen::SparseMatrix<double> exotica::BoundedTimeIndexedProblem::GetEqualityJacobian ( ) const

delete

◆ GetEqualityJacobian() [2/2]

Eigen::MatrixXd exotica::BoundedTimeIndexedProblem::GetEqualityJacobian ( int t ) const

delete

◆ GetEqualityJacobianTriplets()

std::vector<Eigen::Triplet<double> > exotica::BoundedTimeIndexedProblem::GetEqualityJacobianTriplets ( ) const

delete

◆ GetGoalEQ()

Eigen::VectorXd exotica::BoundedTimeIndexedProblem::GetGoalEQ	(	const std::string &	task_name,
		int	t = `0`
	)

delete

◆ GetGoalNEQ()

Eigen::VectorXd exotica::BoundedTimeIndexedProblem::GetGoalNEQ	(	const std::string &	task_name,
		int	t = `0`
	)

delete

◆ GetInequality() [1/2]

Eigen::VectorXd exotica::BoundedTimeIndexedProblem::GetInequality ( ) const

delete

◆ GetInequality() [2/2]

Eigen::VectorXd exotica::BoundedTimeIndexedProblem::GetInequality ( int t ) const

delete

◆ GetInequalityJacobian() [1/2]

Eigen::SparseMatrix<double> exotica::BoundedTimeIndexedProblem::GetInequalityJacobian ( ) const

delete

◆ GetInequalityJacobian() [2/2]

Eigen::MatrixXd exotica::BoundedTimeIndexedProblem::GetInequalityJacobian ( int t ) const

delete

◆ GetInequalityJacobianTriplets()

std::vector<Eigen::Triplet<double> > exotica::BoundedTimeIndexedProblem::GetInequalityJacobianTriplets ( ) const

delete

◆ GetJointVelocityConstraint()

Eigen::VectorXd exotica::BoundedTimeIndexedProblem::GetJointVelocityConstraint ( ) const

delete

◆ GetJointVelocityConstraintBounds()

Eigen::MatrixXd exotica::BoundedTimeIndexedProblem::GetJointVelocityConstraintBounds ( ) const

delete

◆ GetJointVelocityConstraintJacobianTriplets()

std::vector<Eigen::Triplet<double> > exotica::BoundedTimeIndexedProblem::GetJointVelocityConstraintJacobianTriplets ( ) const

delete

◆ GetJointVelocityLimits()

Eigen::VectorXd exotica::BoundedTimeIndexedProblem::GetJointVelocityLimits ( ) const

delete

◆ GetRhoEQ()

double exotica::BoundedTimeIndexedProblem::GetRhoEQ	(	const std::string &	task_name,
		int	t = `0`
	)

delete

◆ GetRhoNEQ()

double exotica::BoundedTimeIndexedProblem::GetRhoNEQ	(	const std::string &	task_name,
		int	t = `0`
	)

delete

◆ Instantiate()

void exotica::BoundedTimeIndexedProblem::Instantiate ( const BoundedTimeIndexedProblemInitializer & init )

overridevirtual

Instantiates the problem from an Initializer.

Reimplemented from exotica::Instantiable< BoundedTimeIndexedProblemInitializer >.

◆ IsValid()

bool exotica::BoundedTimeIndexedProblem::IsValid ( )

overridevirtual

Evaluates whether the problem is valid.

Reimplemented from exotica::PlanningProblem.

◆ PreUpdate()

void exotica::BoundedTimeIndexedProblem::PreUpdate ( )

overridevirtual

Updates internal variables before solving, e.g., after setting new values for Rho.

Reimplemented from exotica::AbstractTimeIndexedProblem.

◆ ReinitializeVariables()

void exotica::BoundedTimeIndexedProblem::ReinitializeVariables ( )

overrideprivatevirtual

Reimplemented from exotica::AbstractTimeIndexedProblem.

◆ SetGoalEQ()

void exotica::BoundedTimeIndexedProblem::SetGoalEQ	(	const std::string &	task_name,
		Eigen::VectorXdRefConst	goal,
		int	t = `0`
	)

delete

◆ SetGoalNEQ()

void exotica::BoundedTimeIndexedProblem::SetGoalNEQ	(	const std::string &	task_name,
		Eigen::VectorXdRefConst	goal,
		int	t = `0`
	)

delete

◆ SetJointVelocityLimits()

void exotica::BoundedTimeIndexedProblem::SetJointVelocityLimits ( const Eigen::VectorXd & qdot_max_in )

delete

◆ SetRhoEQ()

void exotica::BoundedTimeIndexedProblem::SetRhoEQ	(	const std::string &	task_name,
		const double	rho,
		int	t = `0`
	)

delete

◆ SetRhoNEQ()

void exotica::BoundedTimeIndexedProblem::SetRhoNEQ	(	const std::string &	task_name,
		const double	rho,
		int	t = `0`
	)

delete

◆ Update() [1/3]

void exotica::BoundedTimeIndexedProblem::Update	(	Eigen::VectorXdRefConst	x,
		int	t
	)

overridevirtual

Updates an individual timestep from a given state vector.

Parameters

x_in	State
t	Timestep to update

Reimplemented from exotica::AbstractTimeIndexedProblem.

◆ Update() [2/3]

virtual void exotica::AbstractTimeIndexedProblem::Update

Updates an individual timestep from a given state vector.

Parameters

x_in	State
t	Timestep to update

◆ Update() [3/3]

void exotica::AbstractTimeIndexedProblem::Update

Updates the entire problem from a given trajectory (e.g., used in an optimization solver)

Parameters

x_trajectory_in Trajectory flattened as a vector; expects dimension: (T - 1) * N

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

/tmp/exotica/exotica_core/include/exotica_core/problems/bounded_time_indexed_problem.h

Public Member Functions

Private Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ BoundedTimeIndexedProblem()

◆ ~BoundedTimeIndexedProblem()

Member Function Documentation

◆ get_active_nonlinear_equality_constraints_dimension()

◆ get_active_nonlinear_inequality_constraints_dimension()

◆ get_joint_velocity_constraint_dimension()

◆ GetEquality() [1/2]

◆ GetEquality() [2/2]

◆ GetEqualityJacobian() [1/2]

◆ GetEqualityJacobian() [2/2]

◆ GetEqualityJacobianTriplets()

◆ GetGoalEQ()

◆ GetGoalNEQ()

◆ GetInequality() [1/2]

◆ GetInequality() [2/2]

◆ GetInequalityJacobian() [1/2]

◆ GetInequalityJacobian() [2/2]

◆ GetInequalityJacobianTriplets()

◆ GetJointVelocityConstraint()

◆ GetJointVelocityConstraintBounds()

◆ GetJointVelocityConstraintJacobianTriplets()

◆ GetJointVelocityLimits()

◆ GetRhoEQ()

◆ GetRhoNEQ()

◆ Instantiate()

◆ IsValid()

◆ PreUpdate()

◆ ReinitializeVariables()

◆ SetGoalEQ()

◆ SetGoalNEQ()

◆ SetJointVelocityLimits()

◆ SetRhoEQ()

◆ SetRhoNEQ()

◆ Update() [1/3]

◆ Update() [2/3]

◆ Update() [3/3]