Exotica
Public Member Functions | Private Member Functions | List of all members
exotica::BoundedTimeIndexedProblem Class Reference

Bound constrained time-indexed problem. More...

#include <bounded_time_indexed_problem.h>

Inheritance diagram for exotica::BoundedTimeIndexedProblem:
Inheritance graph
Collaboration diagram for exotica::BoundedTimeIndexedProblem:
Collaboration graph

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^T*W*x) 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
 
TaskMapMapGetTaskMaps ()
 
TaskMapVecGetTasks ()
 
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< InitializerGetAllTemplates () const =0
 
- Public Member Functions inherited from exotica::Instantiable< BoundedTimeIndexedProblemInitializer >
void InstantiateInternal (const Initializer &init) override
 
Initializer GetInitializerTemplate () override
 
std::vector< InitializerGetAllTemplates () 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< TaskSpaceVectorPhi
 
std::vector< Eigen::MatrixXd > jacobian
 
std::vector< Hessianhessian
 
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

◆ 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_inState
tTimestep 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_inState
tTimestep 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_inTrajectory flattened as a vector; expects dimension: (T - 1) * N

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