Bound constrained time-indexed problem. More...

#include <bounded_time_indexed_problem.h>

Inheritance diagram for exotica::BoundedTimeIndexedProblem:

[legend]

Public Member Functions
	BoundedTimeIndexedProblem ()=default

int	get_active_nonlinear_equality_constraints_dimension () const =delete

int	get_active_nonlinear_inequality_constraints_dimension () const =delete

int	get_joint_velocity_constraint_dimension () const =delete

Eigen::VectorXd	GetEquality () const =delete

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

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

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

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

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

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

Eigen::VectorXd	GetInequality () const =delete

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

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

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

std::vector< Eigen::Triplet< double > >	GetInequalityJacobianTriplets () 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

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

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

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

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

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

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

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

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

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

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

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

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

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

virtual	~BoundedTimeIndexedProblem ()=default

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

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

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

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

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

Eigen::MatrixXd	GetBounds () const
	Returns the joint bounds (first column lower, second column upper). 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...

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

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

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

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

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

std::vector< Eigen::Triplet< double > >	GetEqualityJacobianTriplets () const
	Returns a vector of triplets to fill a sparse Jacobian for the equality constraints. 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...

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...

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...

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

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

Eigen::SparseMatrix< double >	GetInequalityJacobian () const
	Returns the sparse Jacobian matrix of the inequality constraints over the entire trajectory. 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...

std::vector< Eigen::VectorXd >	GetInitialTrajectory () const
	Returns the initial trajectory/seed. 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...

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...

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...

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...

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...

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

double	GetTau () const
	Returns the time discretization tau for the trajectory. 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...

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...

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...

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

void	SetJointVelocityLimits (const Eigen::VectorXd &qdot_max_in)
	Sets the joint velocity limits. Supports N- and 1-dimensional vectors. 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...

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...

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...

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...

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

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

virtual	~AbstractTimeIndexedProblem ()

Public Member Functions inherited from exotica::PlanningProblem
virtual Eigen::VectorXd	ApplyStartState (bool update_traj=true)

int	get_num_controls () 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

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

double	GetCostEvolution (int index) const

KinematicRequestFlags	GetFlags () const

int	GetNumberOfIterations () const

unsigned int	GetNumberOfProblemUpdates () const

ScenePtr	GetScene () const

Eigen::VectorXd	GetStartState () const

double	GetStartTime () const

TaskMapMap &	GetTaskMaps ()

TaskMapVec &	GetTasks ()

void	InstantiateBase (const Initializer &init) override

	PlanningProblem ()

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

void	ResetCostEvolution (size_t size)

void	ResetNumberOfProblemUpdates ()

void	SetCostEvolution (int index, double value)

void	SetStartState (Eigen::VectorXdRefConst x)

void	SetStartTime (double t)

virtual	~PlanningProblem ()

Public Member Functions inherited from exotica::Object
std::string	GetObjectName ()

void	InstantiateObject (const Initializer &init)

	Object ()

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

virtual	~Object ()

Public Member Functions inherited from exotica::InstantiableBase
virtual std::vector< Initializer >	GetAllTemplates () const =0

	InstantiableBase ()=default

virtual	~InstantiableBase ()=default

Public Member Functions inherited from exotica::Instantiable< BoundedTimeIndexedProblemInitializer >
std::vector< Initializer >	GetAllTemplates () const override

Initializer	GetInitializerTemplate () override

const BoundedTimeIndexedProblemInitializer &	GetParameters () const

void	InstantiateInternal (const Initializer &init) override

Private Member Functions
void	ReinitializeVariables () override

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

TimeIndexedTask	equality
	General equality task. More...

std::vector< Hessian >	hessian

TimeIndexedTask	inequality
	General inequality task. More...

std::vector< Eigen::MatrixXd >	jacobian

int	length_jacobian

int	length_Phi

int	num_tasks

std::vector< TaskSpaceVector >	Phi

bool	use_bounds

Eigen::MatrixXd	W

Public Attributes inherited from exotica::PlanningProblem
int	N = 0

TerminationCriterion	termination_criterion

Public Attributes inherited from exotica::Object
bool	debug_

std::string	ns_

std::string	object_name_

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	UpdateMultipleTaskKinematics (std::vector< std::shared_ptr< KinematicResponse >> responses)

void	UpdateTaskKinematics (std::shared_ptr< KinematicResponse > response)

Protected Attributes inherited from exotica::AbstractTimeIndexedProblem
std::vector< std::pair< int, int > >	active_nonlinear_equality_constraints_

int	active_nonlinear_equality_constraints_dimension_ = 0

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

int	active_nonlinear_inequality_constraints_dimension_ = 0

TaskSpaceVector	cost_Phi

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

TaskSpaceVector	equality_Phi

TaskSpaceVector	inequality_Phi

std::vector< Eigen::VectorXd >	initial_trajectory_

int	joint_velocity_constraint_dimension_ = 0

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

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

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

int	T_ = 0
	Number of time steps. More...

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

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

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

std::vector< Eigen::VectorXd >	xdiff

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

Protected Attributes inherited from exotica::PlanningProblem
std::vector< std::pair< std::chrono::high_resolution_clock::time_point, double > >	cost_evolution_

KinematicRequestFlags	flags_ = KinematicRequestFlags::KIN_FK

unsigned int	number_of_problem_updates_ {0}

ScenePtr	scene_

Eigen::VectorXd	start_state_

double	t_start {0.0}

TaskMapMap	task_maps_

TaskMapVec	tasks_

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

Detailed Description

Bound constrained time-indexed problem.

Definition at line 39 of file bounded_time_indexed_problem.h.

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 >.

Definition at line 37 of file bounded_time_indexed_problem.cpp.

◆ IsValid()

bool exotica::BoundedTimeIndexedProblem::IsValid ( )

overridevirtual

Evaluates whether the problem is valid.

Reimplemented from exotica::PlanningProblem.

Definition at line 191 of file bounded_time_indexed_problem.cpp.

◆ PreUpdate()

void exotica::BoundedTimeIndexedProblem::PreUpdate ( )

overridevirtual

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

Reimplemented from exotica::AbstractTimeIndexedProblem.

Definition at line 66 of file bounded_time_indexed_problem.cpp.

◆ ReinitializeVariables()

void exotica::BoundedTimeIndexedProblem::ReinitializeVariables ( )

overrideprivatevirtual

Reimplemented from exotica::AbstractTimeIndexedProblem.

Definition at line 151 of file bounded_time_indexed_problem.cpp.

◆ 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.

Definition at line 80 of file bounded_time_indexed_problem.cpp.

◆ Update() [2/3]

void exotica::AbstractTimeIndexedProblem::Update

Updates an individual timestep from a given state vector.

Parameters

x_in	State
t	Timestep to update

Definition at line 220 of file abstract_time_indexed_problem.cpp.

◆ 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

Definition at line 209 of file abstract_time_indexed_problem.cpp.

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

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]