#include <time_indexed_sampling_problem.h>
Public Member Functions | |
std::vector< double > | GetBounds () |
Eigen::VectorXd | GetGoalEQ (const std::string &task_name) |
Eigen::VectorXd | GetGoalNEQ (const std::string &task_name) |
Eigen::VectorXd | GetGoalState () const |
double | GetGoalTime () const |
double | GetRhoEQ (const std::string &task_name) |
double | GetRhoNEQ (const std::string &task_name) |
int | GetSpaceDim () |
void | Instantiate (const TimeIndexedSamplingProblemInitializer &init) override |
bool | IsValid (Eigen::VectorXdRefConst x, const double &t) |
void | PreUpdate () override |
void | SetGoalEQ (const std::string &task_name, Eigen::VectorXdRefConst goal) |
void | SetGoalNEQ (const std::string &task_name, Eigen::VectorXdRefConst goal) |
void | SetGoalState (Eigen::VectorXdRefConst qT) |
void | SetGoalTime (const double &t) |
void | SetRhoEQ (const std::string &task_name, const double &rho) |
void | SetRhoNEQ (const std::string &task_name, const double &rho) |
TimeIndexedSamplingProblem () | |
void | Update (Eigen::VectorXdRefConst x, const double &t) |
virtual | ~TimeIndexedSamplingProblem () |
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 |
virtual bool | IsValid () |
Evaluates whether the problem is valid. More... | |
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 | |
InstantiableBase ()=default | |
virtual | ~InstantiableBase ()=default |
Public Member Functions inherited from exotica::Instantiable< TimeIndexedSamplingProblemInitializer > | |
std::vector< Initializer > | GetAllTemplates () const override |
Initializer | GetInitializerTemplate () override |
const TimeIndexedSamplingProblemInitializer & | GetParameters () const |
void | InstantiateInternal (const Initializer &init) override |
Public Attributes | |
TaskSpaceVector | constraint_phi |
SamplingTask | equality |
SamplingTask | inequality |
int | length_jacobian |
int | length_Phi |
int | num_tasks |
TimeIndexedSamplingProblemInitializer | parameters |
TaskSpaceVector | Phi |
Eigen::VectorXd | vel_limits |
Public Attributes inherited from exotica::PlanningProblem | |
int | N = 0 |
double | t_start |
TerminationCriterion | termination_criterion |
Public Attributes inherited from exotica::Object | |
bool | debug_ |
std::string | ns_ |
std::string | object_name_ |
Private Attributes | |
Eigen::VectorXd | goal_ |
Goal state to reach (spatial) at temporal goal (t_goal_) More... | |
double | t_goal_ |
Goal time: The time at which goal_ should be reached and the upper bound for the time-dimension. More... | |
Additional Inherited Members | |
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::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_ |
TaskMapMap | task_maps_ |
TaskMapVec | tasks_ |
Protected Attributes inherited from exotica::Instantiable< TimeIndexedSamplingProblemInitializer > | |
TimeIndexedSamplingProblemInitializer | parameters_ |
Definition at line 40 of file time_indexed_sampling_problem.h.
exotica::TimeIndexedSamplingProblem::TimeIndexedSamplingProblem | ( | ) |
Definition at line 37 of file time_indexed_sampling_problem.cpp.
|
virtualdefault |
std::vector< double > exotica::TimeIndexedSamplingProblem::GetBounds | ( | ) |
Definition at line 44 of file time_indexed_sampling_problem.cpp.
Eigen::VectorXd exotica::TimeIndexedSamplingProblem::GetGoalEQ | ( | const std::string & | task_name | ) |
Definition at line 184 of file time_indexed_sampling_problem.cpp.
Eigen::VectorXd exotica::TimeIndexedSamplingProblem::GetGoalNEQ | ( | const std::string & | task_name | ) |
Definition at line 236 of file time_indexed_sampling_problem.cpp.
Eigen::VectorXd exotica::TimeIndexedSamplingProblem::GetGoalState | ( | ) | const |
Definition at line 144 of file time_indexed_sampling_problem.cpp.
double exotica::TimeIndexedSamplingProblem::GetGoalTime | ( | ) | const |
Definition at line 134 of file time_indexed_sampling_problem.cpp.
double exotica::TimeIndexedSamplingProblem::GetRhoEQ | ( | const std::string & | task_name | ) |
Definition at line 196 of file time_indexed_sampling_problem.cpp.
double exotica::TimeIndexedSamplingProblem::GetRhoNEQ | ( | const std::string & | task_name | ) |
Definition at line 248 of file time_indexed_sampling_problem.cpp.
int exotica::TimeIndexedSamplingProblem::GetSpaceDim | ( | ) |
Definition at line 297 of file time_indexed_sampling_problem.cpp.
|
overridevirtual |
Reimplemented from exotica::Instantiable< TimeIndexedSamplingProblemInitializer >.
Definition at line 59 of file time_indexed_sampling_problem.cpp.
bool exotica::TimeIndexedSamplingProblem::IsValid | ( | Eigen::VectorXdRefConst | x, |
const double & | t | ||
) |
Definition at line 260 of file time_indexed_sampling_problem.cpp.
|
overridevirtual |
Reimplemented from exotica::PlanningProblem.
Definition at line 283 of file time_indexed_sampling_problem.cpp.
void exotica::TimeIndexedSamplingProblem::SetGoalEQ | ( | const std::string & | task_name, |
Eigen::VectorXdRefConst | goal | ||
) |
Definition at line 156 of file time_indexed_sampling_problem.cpp.
void exotica::TimeIndexedSamplingProblem::SetGoalNEQ | ( | const std::string & | task_name, |
Eigen::VectorXdRefConst | goal | ||
) |
Definition at line 208 of file time_indexed_sampling_problem.cpp.
void exotica::TimeIndexedSamplingProblem::SetGoalState | ( | Eigen::VectorXdRefConst | qT | ) |
Definition at line 149 of file time_indexed_sampling_problem.cpp.
void exotica::TimeIndexedSamplingProblem::SetGoalTime | ( | const double & | t | ) |
Definition at line 139 of file time_indexed_sampling_problem.cpp.
void exotica::TimeIndexedSamplingProblem::SetRhoEQ | ( | const std::string & | task_name, |
const double & | rho | ||
) |
Definition at line 170 of file time_indexed_sampling_problem.cpp.
void exotica::TimeIndexedSamplingProblem::SetRhoNEQ | ( | const std::string & | task_name, |
const double & | rho | ||
) |
Definition at line 222 of file time_indexed_sampling_problem.cpp.
void exotica::TimeIndexedSamplingProblem::Update | ( | Eigen::VectorXdRefConst | x, |
const double & | t | ||
) |
Definition at line 291 of file time_indexed_sampling_problem.cpp.
TaskSpaceVector exotica::TimeIndexedSamplingProblem::constraint_phi |
Definition at line 78 of file time_indexed_sampling_problem.h.
SamplingTask exotica::TimeIndexedSamplingProblem::equality |
Definition at line 76 of file time_indexed_sampling_problem.h.
|
private |
Goal state to reach (spatial) at temporal goal (t_goal_)
Definition at line 86 of file time_indexed_sampling_problem.h.
SamplingTask exotica::TimeIndexedSamplingProblem::inequality |
Definition at line 75 of file time_indexed_sampling_problem.h.
int exotica::TimeIndexedSamplingProblem::length_jacobian |
Definition at line 81 of file time_indexed_sampling_problem.h.
int exotica::TimeIndexedSamplingProblem::length_Phi |
Definition at line 80 of file time_indexed_sampling_problem.h.
int exotica::TimeIndexedSamplingProblem::num_tasks |
Definition at line 82 of file time_indexed_sampling_problem.h.
TimeIndexedSamplingProblemInitializer exotica::TimeIndexedSamplingProblem::parameters |
Definition at line 77 of file time_indexed_sampling_problem.h.
TaskSpaceVector exotica::TimeIndexedSamplingProblem::Phi |
Definition at line 74 of file time_indexed_sampling_problem.h.
|
private |
Goal time: The time at which goal_ should be reached and the upper bound for the time-dimension.
Definition at line 85 of file time_indexed_sampling_problem.h.
Eigen::VectorXd exotica::TimeIndexedSamplingProblem::vel_limits |
Definition at line 73 of file time_indexed_sampling_problem.h.