Exotica
Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes | List of all members
exotica::DynamicTimeIndexedShootingProblem Class Reference

#include <dynamic_time_indexed_shooting_problem.h>

Inheritance diagram for exotica::DynamicTimeIndexedShootingProblem:
Inheritance graph
Collaboration diagram for exotica::DynamicTimeIndexedShootingProblem:
Collaboration graph

Public Member Functions

 DynamicTimeIndexedShootingProblem ()
 
virtual ~DynamicTimeIndexedShootingProblem ()
 
void Instantiate (const DynamicTimeIndexedShootingProblemInitializer &init) override
 
Eigen::VectorXd ApplyStartState (bool update_traj=true) override
 
void PreUpdate () override
 
void Update (Eigen::VectorXdRefConst u, int t)
 
void Update (Eigen::VectorXdRefConst x, Eigen::VectorXdRefConst u, int t)
 
void UpdateTerminalState (Eigen::VectorXdRefConst x)
 
const int & get_T () const
 Returns the number of timesteps in the state trajectory. More...
 
void set_T (const int &T_in)
 Sets the number of timesteps in the state trajectory. More...
 
const double & get_tau () const
 Returns the discretization timestep tau. More...
 
const Eigen::MatrixXd & get_X () const
 Returns the state trajectory X. More...
 
Eigen::VectorXd get_X (int t) const
 Returns the state at time t. More...
 
void set_X (Eigen::MatrixXdRefConst X_in)
 Sets the state trajectory X (can be used as the initial guess) More...
 
const Eigen::MatrixXd & get_U () const
 Returns the control trajectory U. More...
 
Eigen::VectorXd get_U (int t) const
 Returns the control state at time t. More...
 
void set_U (Eigen::MatrixXdRefConst U_in)
 Sets the control trajectory U (can be used as the initial guess) More...
 
const Eigen::MatrixXd & get_X_star () const
 Returns the target state trajectory X. More...
 
void set_X_star (Eigen::MatrixXdRefConst X_star_in)
 Sets the target state trajectory X. More...
 
const Eigen::MatrixXd & get_Q (int t) const
 Returns the precision matrix at time step t. More...
 
void set_Q (Eigen::MatrixXdRefConst Q_in, int t)
 Sets the precision matrix for time step t. More...
 
const Eigen::MatrixXd & get_Qf () const
 Returns the cost weight matrix at time N. More...
 
void set_Qf (Eigen::MatrixXdRefConst Q_in)
 Sets the cost weight matrix for time N. More...
 
const Eigen::MatrixXd & get_R () const
 Returns the control weight matrix. More...
 
Eigen::MatrixXd get_F (int t) const
 Returns the noise weight matrix at time t. More...
 
const Eigen::MatrixXd & GetControlNoiseJacobian (int column_idx) const
 F[i]_u. More...
 
void EnableStochasticUpdates ()
 
void DisableStochasticUpdates ()
 
double GetStateCost (int t) const
 
double GetControlCost (int t) const
 
Eigen::VectorXd GetStateCostJacobian (int t)
 lx More...
 
Eigen::VectorXd GetControlCostJacobian (int t)
 lu More...
 
Eigen::MatrixXd GetStateCostHessian (int t)
 lxx More...
 
Eigen::MatrixXd GetControlCostHessian (int t)
 luu More...
 
Eigen::MatrixXd GetStateControlCostHessian ()
 
void OnSolverIterationEnd ()
 lxu == lux More...
 
const ControlCostLossTermTypeget_loss_type () const
 
void set_loss_type (const ControlCostLossTermType &loss_type_in)
 
double get_control_cost_weight () const
 
void set_control_cost_weight (const double control_cost_weight_in)
 
- 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
 
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
 
virtual bool IsValid ()
 Evaluates whether the problem is valid. More...
 
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< DynamicTimeIndexedShootingProblemInitializer >
void InstantiateInternal (const Initializer &init) override
 
Initializer GetInitializerTemplate () override
 
std::vector< InitializerGetAllTemplates () const override
 
const DynamicTimeIndexedShootingProblemInitializer & GetParameters () const
 

Public Attributes

TimeIndexedTask cost
 Cost task. More...
 
std::vector< TaskSpaceVectorPhi
 Stacked TaskMap vector. More...
 
std::vector< Eigen::MatrixXd > dPhi_dx
 Stacked TaskMap Jacobian w.r.t. state. More...
 
std::vector< Eigen::MatrixXd > dPhi_du
 Stacked TaskMap Jacobian w.r.t. control. More...
 
std::vector< HessianddPhi_ddx
 Stacked TaskMap Hessian w.r.t. state. More...
 
std::vector< HessianddPhi_ddu
 Stacked TaskMap Hessian w.r.t. control. More...
 
std::vector< HessianddPhi_dxdu
 Stacked TaskMap Hessian w.r.t. state and control. More...
 
int length_Phi
 Length of TaskSpaceVector (Phi => stacked task-maps) More...
 
int length_jacobian
 Length of tangent vector to Phi. More...
 
int num_tasks
 Number of TaskMaps. More...
 
- 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

void ValidateTimeIndex (int &t_in) const
 Checks the desired time index for bounds and supports -1 indexing. More...
 
void ReinitializeVariables ()
 
void UpdateTaskMaps (Eigen::VectorXdRefConst x, Eigen::VectorXdRefConst u, int t)
 
void InstantiateCostTerms (const DynamicTimeIndexedShootingProblemInitializer &init)
 
- 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

int T_
 Number of time steps. More...
 
double tau_
 Time step duration. More...
 
bool stochastic_matrices_specified_ = false
 
bool stochastic_updates_enabled_ = false
 
Eigen::MatrixXd X_
 State trajectory (i.e., positions, velocities). Size: num-states x T. More...
 
Eigen::MatrixXd U_
 Control trajectory. Size: num-controls x (T-1) More...
 
Eigen::MatrixXd X_star_
 Goal state trajectory (i.e., positions, velocities). Size: num-states x T. More...
 
Eigen::MatrixXd X_diff_
 Difference between X_ and X_star_. Size: ndx x T. More...
 
Eigen::MatrixXd Qf_
 Final state cost. More...
 
std::vector< Eigen::MatrixXd > Q_
 State space penalty matrix (precision matrix), per time index. More...
 
Eigen::MatrixXd R_
 Control space penalty matrix. More...
 
std::vector< Eigen::MatrixXd > Ci_
 Noise weight terms. More...
 
Eigen::MatrixXd CW_
 White noise covariance. More...
 
std::vector< Eigen::MatrixXd > dxdiff_
 
std::vector< Eigen::VectorXd > state_cost_jacobian_
 
std::vector< Eigen::MatrixXd > state_cost_hessian_
 
std::vector< Eigen::VectorXd > general_cost_jacobian_
 
std::vector< Eigen::MatrixXd > general_cost_hessian_
 
std::vector< Eigen::VectorXd > control_cost_jacobian_
 
std::vector< Eigen::MatrixXd > control_cost_hessian_
 
std::vector< std::shared_ptr< KinematicResponse > > kinematic_solutions_
 
std::mt19937 generator_
 
std::normal_distribution< double > standard_normal_noise_ {0, 1}
 
TaskSpaceVector cost_Phi
 
double control_cost_weight_ = 1
 
ControlCostLossTermType loss_type_
 
Eigen::VectorXd l1_rate_
 
Eigen::VectorXd huber_rate_
 
Eigen::VectorXd bimodal_huber_mode1_
 
Eigen::VectorXd bimodal_huber_mode2_
 
Eigen::VectorXd smooth_l1_mean_
 
Eigen::VectorXd smooth_l1_std_
 
- 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< DynamicTimeIndexedShootingProblemInitializer >
DynamicTimeIndexedShootingProblemInitializer parameters_
 

Constructor & Destructor Documentation

◆ DynamicTimeIndexedShootingProblem()

exotica::DynamicTimeIndexedShootingProblem::DynamicTimeIndexedShootingProblem ( )

◆ ~DynamicTimeIndexedShootingProblem()

virtual exotica::DynamicTimeIndexedShootingProblem::~DynamicTimeIndexedShootingProblem ( )
virtual

Member Function Documentation

◆ ApplyStartState()

Eigen::VectorXd exotica::DynamicTimeIndexedShootingProblem::ApplyStartState ( bool  update_traj = true)
overridevirtual

Reimplemented from exotica::PlanningProblem.

◆ DisableStochasticUpdates()

void exotica::DynamicTimeIndexedShootingProblem::DisableStochasticUpdates ( )

◆ EnableStochasticUpdates()

void exotica::DynamicTimeIndexedShootingProblem::EnableStochasticUpdates ( )

◆ get_control_cost_weight()

double exotica::DynamicTimeIndexedShootingProblem::get_control_cost_weight ( ) const
inline

◆ get_F()

Eigen::MatrixXd exotica::DynamicTimeIndexedShootingProblem::get_F ( int  t) const

Returns the noise weight matrix at time t.

◆ get_loss_type()

const ControlCostLossTermType& exotica::DynamicTimeIndexedShootingProblem::get_loss_type ( ) const
inline

◆ get_Q()

const Eigen::MatrixXd& exotica::DynamicTimeIndexedShootingProblem::get_Q ( int  t) const

Returns the precision matrix at time step t.

◆ get_Qf()

const Eigen::MatrixXd& exotica::DynamicTimeIndexedShootingProblem::get_Qf ( ) const

Returns the cost weight matrix at time N.

◆ get_R()

const Eigen::MatrixXd& exotica::DynamicTimeIndexedShootingProblem::get_R ( ) const

Returns the control weight matrix.

◆ get_T()

const int& exotica::DynamicTimeIndexedShootingProblem::get_T ( ) const

Returns the number of timesteps in the state trajectory.

◆ get_tau()

const double& exotica::DynamicTimeIndexedShootingProblem::get_tau ( ) const

Returns the discretization timestep tau.

◆ get_U() [1/2]

const Eigen::MatrixXd& exotica::DynamicTimeIndexedShootingProblem::get_U ( ) const

Returns the control trajectory U.

◆ get_U() [2/2]

Eigen::VectorXd exotica::DynamicTimeIndexedShootingProblem::get_U ( int  t) const

Returns the control state at time t.

◆ get_X() [1/2]

const Eigen::MatrixXd& exotica::DynamicTimeIndexedShootingProblem::get_X ( ) const

Returns the state trajectory X.

◆ get_X() [2/2]

Eigen::VectorXd exotica::DynamicTimeIndexedShootingProblem::get_X ( int  t) const

Returns the state at time t.

◆ get_X_star()

const Eigen::MatrixXd& exotica::DynamicTimeIndexedShootingProblem::get_X_star ( ) const

Returns the target state trajectory X.

◆ GetControlCost()

double exotica::DynamicTimeIndexedShootingProblem::GetControlCost ( int  t) const

◆ GetControlCostHessian()

Eigen::MatrixXd exotica::DynamicTimeIndexedShootingProblem::GetControlCostHessian ( int  t)

luu

◆ GetControlCostJacobian()

Eigen::VectorXd exotica::DynamicTimeIndexedShootingProblem::GetControlCostJacobian ( int  t)

lu

◆ GetControlNoiseJacobian()

const Eigen::MatrixXd& exotica::DynamicTimeIndexedShootingProblem::GetControlNoiseJacobian ( int  column_idx) const

F[i]_u.

◆ GetStateControlCostHessian()

Eigen::MatrixXd exotica::DynamicTimeIndexedShootingProblem::GetStateControlCostHessian ( )
inline

◆ GetStateCost()

double exotica::DynamicTimeIndexedShootingProblem::GetStateCost ( int  t) const

◆ GetStateCostHessian()

Eigen::MatrixXd exotica::DynamicTimeIndexedShootingProblem::GetStateCostHessian ( int  t)

lxx

◆ GetStateCostJacobian()

Eigen::VectorXd exotica::DynamicTimeIndexedShootingProblem::GetStateCostJacobian ( int  t)

lx

◆ Instantiate()

void exotica::DynamicTimeIndexedShootingProblem::Instantiate ( const DynamicTimeIndexedShootingProblemInitializer &  init)
overridevirtual

◆ InstantiateCostTerms()

void exotica::DynamicTimeIndexedShootingProblem::InstantiateCostTerms ( const DynamicTimeIndexedShootingProblemInitializer &  init)
protected

◆ OnSolverIterationEnd()

void exotica::DynamicTimeIndexedShootingProblem::OnSolverIterationEnd ( )
inline

lxu == lux

◆ PreUpdate()

void exotica::DynamicTimeIndexedShootingProblem::PreUpdate ( )
overridevirtual

Reimplemented from exotica::PlanningProblem.

◆ ReinitializeVariables()

void exotica::DynamicTimeIndexedShootingProblem::ReinitializeVariables ( )
protected

◆ set_control_cost_weight()

void exotica::DynamicTimeIndexedShootingProblem::set_control_cost_weight ( const double  control_cost_weight_in)
inline

◆ set_loss_type()

void exotica::DynamicTimeIndexedShootingProblem::set_loss_type ( const ControlCostLossTermType loss_type_in)
inline

◆ set_Q()

void exotica::DynamicTimeIndexedShootingProblem::set_Q ( Eigen::MatrixXdRefConst  Q_in,
int  t 
)

Sets the precision matrix for time step t.

◆ set_Qf()

void exotica::DynamicTimeIndexedShootingProblem::set_Qf ( Eigen::MatrixXdRefConst  Q_in)

Sets the cost weight matrix for time N.

◆ set_T()

void exotica::DynamicTimeIndexedShootingProblem::set_T ( const int &  T_in)

Sets the number of timesteps in the state trajectory.

◆ set_U()

void exotica::DynamicTimeIndexedShootingProblem::set_U ( Eigen::MatrixXdRefConst  U_in)

Sets the control trajectory U (can be used as the initial guess)

◆ set_X()

void exotica::DynamicTimeIndexedShootingProblem::set_X ( Eigen::MatrixXdRefConst  X_in)

Sets the state trajectory X (can be used as the initial guess)

◆ set_X_star()

void exotica::DynamicTimeIndexedShootingProblem::set_X_star ( Eigen::MatrixXdRefConst  X_star_in)

Sets the target state trajectory X.

◆ Update() [1/2]

void exotica::DynamicTimeIndexedShootingProblem::Update ( Eigen::VectorXdRefConst  u,
int  t 
)

◆ Update() [2/2]

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

◆ UpdateTaskMaps()

void exotica::DynamicTimeIndexedShootingProblem::UpdateTaskMaps ( Eigen::VectorXdRefConst  x,
Eigen::VectorXdRefConst  u,
int  t 
)
protected

◆ UpdateTerminalState()

void exotica::DynamicTimeIndexedShootingProblem::UpdateTerminalState ( Eigen::VectorXdRefConst  x)

◆ ValidateTimeIndex()

void exotica::DynamicTimeIndexedShootingProblem::ValidateTimeIndex ( int &  t_in) const
inlineprotected

Checks the desired time index for bounds and supports -1 indexing.

Member Data Documentation

◆ bimodal_huber_mode1_

Eigen::VectorXd exotica::DynamicTimeIndexedShootingProblem::bimodal_huber_mode1_
protected

◆ bimodal_huber_mode2_

Eigen::VectorXd exotica::DynamicTimeIndexedShootingProblem::bimodal_huber_mode2_
protected

◆ Ci_

std::vector<Eigen::MatrixXd> exotica::DynamicTimeIndexedShootingProblem::Ci_
protected

Noise weight terms.

◆ control_cost_hessian_

std::vector<Eigen::MatrixXd> exotica::DynamicTimeIndexedShootingProblem::control_cost_hessian_
protected

◆ control_cost_jacobian_

std::vector<Eigen::VectorXd> exotica::DynamicTimeIndexedShootingProblem::control_cost_jacobian_
protected

◆ control_cost_weight_

double exotica::DynamicTimeIndexedShootingProblem::control_cost_weight_ = 1
protected

◆ cost

TimeIndexedTask exotica::DynamicTimeIndexedShootingProblem::cost

Cost task.

◆ cost_Phi

TaskSpaceVector exotica::DynamicTimeIndexedShootingProblem::cost_Phi
protected

◆ CW_

Eigen::MatrixXd exotica::DynamicTimeIndexedShootingProblem::CW_
protected

White noise covariance.

◆ ddPhi_ddu

std::vector<Hessian> exotica::DynamicTimeIndexedShootingProblem::ddPhi_ddu

Stacked TaskMap Hessian w.r.t. control.

◆ ddPhi_ddx

std::vector<Hessian> exotica::DynamicTimeIndexedShootingProblem::ddPhi_ddx

Stacked TaskMap Hessian w.r.t. state.

◆ ddPhi_dxdu

std::vector<Hessian> exotica::DynamicTimeIndexedShootingProblem::ddPhi_dxdu

Stacked TaskMap Hessian w.r.t. state and control.

◆ dPhi_du

std::vector<Eigen::MatrixXd> exotica::DynamicTimeIndexedShootingProblem::dPhi_du

Stacked TaskMap Jacobian w.r.t. control.

◆ dPhi_dx

std::vector<Eigen::MatrixXd> exotica::DynamicTimeIndexedShootingProblem::dPhi_dx

Stacked TaskMap Jacobian w.r.t. state.

◆ dxdiff_

std::vector<Eigen::MatrixXd> exotica::DynamicTimeIndexedShootingProblem::dxdiff_
protected

◆ general_cost_hessian_

std::vector<Eigen::MatrixXd> exotica::DynamicTimeIndexedShootingProblem::general_cost_hessian_
protected

◆ general_cost_jacobian_

std::vector<Eigen::VectorXd> exotica::DynamicTimeIndexedShootingProblem::general_cost_jacobian_
protected

◆ generator_

std::mt19937 exotica::DynamicTimeIndexedShootingProblem::generator_
protected

◆ huber_rate_

Eigen::VectorXd exotica::DynamicTimeIndexedShootingProblem::huber_rate_
protected

◆ kinematic_solutions_

std::vector<std::shared_ptr<KinematicResponse> > exotica::DynamicTimeIndexedShootingProblem::kinematic_solutions_
protected

◆ l1_rate_

Eigen::VectorXd exotica::DynamicTimeIndexedShootingProblem::l1_rate_
protected

◆ length_jacobian

int exotica::DynamicTimeIndexedShootingProblem::length_jacobian

Length of tangent vector to Phi.

◆ length_Phi

int exotica::DynamicTimeIndexedShootingProblem::length_Phi

Length of TaskSpaceVector (Phi => stacked task-maps)

◆ loss_type_

ControlCostLossTermType exotica::DynamicTimeIndexedShootingProblem::loss_type_
protected

◆ num_tasks

int exotica::DynamicTimeIndexedShootingProblem::num_tasks

Number of TaskMaps.

◆ Phi

std::vector<TaskSpaceVector> exotica::DynamicTimeIndexedShootingProblem::Phi

Stacked TaskMap vector.

◆ Q_

std::vector<Eigen::MatrixXd> exotica::DynamicTimeIndexedShootingProblem::Q_
protected

State space penalty matrix (precision matrix), per time index.

◆ Qf_

Eigen::MatrixXd exotica::DynamicTimeIndexedShootingProblem::Qf_
protected

Final state cost.

◆ R_

Eigen::MatrixXd exotica::DynamicTimeIndexedShootingProblem::R_
protected

Control space penalty matrix.

◆ smooth_l1_mean_

Eigen::VectorXd exotica::DynamicTimeIndexedShootingProblem::smooth_l1_mean_
protected

◆ smooth_l1_std_

Eigen::VectorXd exotica::DynamicTimeIndexedShootingProblem::smooth_l1_std_
protected

◆ standard_normal_noise_

std::normal_distribution<double> exotica::DynamicTimeIndexedShootingProblem::standard_normal_noise_ {0, 1}
protected

◆ state_cost_hessian_

std::vector<Eigen::MatrixXd> exotica::DynamicTimeIndexedShootingProblem::state_cost_hessian_
protected

◆ state_cost_jacobian_

std::vector<Eigen::VectorXd> exotica::DynamicTimeIndexedShootingProblem::state_cost_jacobian_
protected

◆ stochastic_matrices_specified_

bool exotica::DynamicTimeIndexedShootingProblem::stochastic_matrices_specified_ = false
protected

◆ stochastic_updates_enabled_

bool exotica::DynamicTimeIndexedShootingProblem::stochastic_updates_enabled_ = false
protected

◆ T_

int exotica::DynamicTimeIndexedShootingProblem::T_
protected

Number of time steps.

◆ tau_

double exotica::DynamicTimeIndexedShootingProblem::tau_
protected

Time step duration.

◆ U_

Eigen::MatrixXd exotica::DynamicTimeIndexedShootingProblem::U_
protected

Control trajectory. Size: num-controls x (T-1)

◆ X_

Eigen::MatrixXd exotica::DynamicTimeIndexedShootingProblem::X_
protected

State trajectory (i.e., positions, velocities). Size: num-states x T.

◆ X_diff_

Eigen::MatrixXd exotica::DynamicTimeIndexedShootingProblem::X_diff_
protected

Difference between X_ and X_star_. Size: ndx x T.

◆ X_star_

Eigen::MatrixXd exotica::DynamicTimeIndexedShootingProblem::X_star_
protected

Goal state trajectory (i.e., positions, velocities). Size: num-states x T.


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