30 #ifndef EXOTICA_CORE_ABSTRACT_TIME_INDEXED_PROBLEM_H_ 31 #define EXOTICA_CORE_ABSTRACT_TIME_INDEXED_PROBLEM_H_ 33 #include <Eigen/Sparse> 77 Eigen::VectorXd
GetGoal(
const std::string& task_name,
int t = 0);
83 void SetRho(
const std::string& task_name,
const double rho,
int t = 0);
88 double GetRho(
const std::string& task_name,
int t = 0);
99 Eigen::VectorXd
GetGoalEQ(
const std::string& task_name,
int t = 0);
105 void SetRhoEQ(
const std::string& task_name,
const double rho,
int t = 0);
110 double GetRhoEQ(
const std::string& task_name,
int t = 0);
121 Eigen::VectorXd
GetGoalNEQ(
const std::string& task_name,
int t = 0);
127 void SetRhoNEQ(
const std::string& task_name,
const double rho,
int t = 0);
132 double GetRhoNEQ(
const std::string& task_name,
int t = 0);
141 void SetT(
const int T_in);
147 void SetTau(
const double tau_in);
231 std::vector<TaskSpaceVector>
Phi;
247 if (t_in >=
T_ || t_in < -1)
249 ThrowPretty(
"Requested t=" << t_in <<
" out of range, needs to be 0 =< t < " <<
T_);
260 std::vector<Eigen::VectorXd>
x;
289 #endif // EXOTICA_CORE_ABSTRACT_TIME_INDEXED_PROBLEM_H_ 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).
AbstractTimeIndexedProblem()
Eigen::VectorXd GetInequality() const
Returns the inequality constraint values for the entire trajectory.
void Update(Eigen::VectorXdRefConst x_trajectory_in)
Updates the entire problem from a given trajectory (e.g., used in an optimization solver) ...
std::vector< Eigen::Triplet< double > > GetJointVelocityConstraintJacobianTriplets() const
Returns the joint velocity constraint Jacobian as triplets.
std::vector< std::pair< int, int > > active_nonlinear_equality_constraints_
void SetT(const int T_in)
Sets the number of timesteps in the trajectory. Note: Rho/Goal need to be updated for every timestep ...
TimeIndexedTask equality
General equality task.
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).
std::vector< Eigen::Triplet< double > > GetEqualityJacobianTriplets() const
Returns a vector of triplets to fill a sparse Jacobian for the equality constraints.
int joint_velocity_constraint_dimension_
Eigen::MatrixXd GetJointVelocityConstraintBounds() const
Returns the joint velocity constraint bounds (constant terms).
std::vector< Eigen::VectorXd > GetInitialTrajectory() const
Returns the initial trajectory/seed.
double GetRho(const std::string &task_name, int t=0)
Returns the precision (Rho) for a given task at a given timestep (cost task).
int get_joint_velocity_constraint_dimension() const
Returns the dimension of the joint velocity constraint (linear inequality).
double GetTau() const
Returns the time discretization tau for the trajectory.
std::vector< Eigen::VectorXd > xdiff
Eigen::VectorXd GetGoalEQ(const std::string &task_name, int t=0)
Returns the goal for a given task at a given timestep (equality task).
TaskSpaceVector equality_Phi
std::vector< std::pair< int, int > > active_nonlinear_inequality_constraints_
std::vector< Eigen::Triplet< double > > joint_velocity_constraint_jacobian_triplets_
geometry_msgs::TransformStamped t
std::vector< Eigen::Triplet< double > > GetInequalityJacobianTriplets() const
Returns a vector of triplets to fill a sparse Jacobian for the inequality constraints.
virtual void ReinitializeVariables()
Eigen::VectorXd GetGoalNEQ(const std::string &task_name, int t=0)
Returns the goal for a given task at a given timestep (goal task).
int active_nonlinear_inequality_constraints_dimension_
Eigen::VectorXd GetJointVelocityLimits() const
Returns the per-DoF joint velocity limit vector.
double GetScalarTransitionCost(int t) const
Returns the scalar transition cost (x^T*W*x) at timestep t.
int get_active_nonlinear_inequality_constraints_dimension() const
Returns the dimension of the active inequality constraints.
TimeIndexedTask inequality
General inequality task.
double GetScalarTaskCost(int t) const
Returns the scalar task cost at timestep t.
std::vector< Hessian > hessian
double get_ct() const
Returns the cost scaling factor.
double GetRhoEQ(const std::string &task_name, int t=0)
Returns the precision (Rho) for a given task at a given timestep (equality task). ...
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).
std::vector< Eigen::VectorXd > x
Current internal problem state.
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).
Eigen::RowVectorXd GetScalarTransitionJacobian(int t) const
Returns the Jacobian of the transition cost at timestep t.
double w_scale_
Kinematic system transition error covariance multiplier (constant throughout the trajectory) ...
void ValidateTimeIndex(int &t_in) const
Checks the desired time index for bounds and supports -1 indexing.
virtual ~AbstractTimeIndexedProblem()
Eigen::VectorXd GetGoal(const std::string &task_name, int t=0)
Returns the goal for a given task at a given timestep (cost task).
int T_
Number of time steps.
double GetDuration() const
Returns the duration of the trajectory (T * tau).
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Convenience wrapper for storing references to sub-matrices/vectors.
void SetJointVelocityLimits(const Eigen::VectorXd &qdot_max_in)
Sets the joint velocity limits. Supports N- and 1-dimensional vectors.
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).
double GetRhoNEQ(const std::string &task_name, int t=0)
Returns the precision (Rho) for a given task at a given timestep (equality task). ...
TimeIndexedTask cost
Cost task.
double GetCost() const
Returns the scalar cost for the entire trajectory (both task and transition cost).
std::vector< Eigen::MatrixXd > jacobian
int GetT() const
Returns the number of timesteps in the trajectory.
TaskSpaceVector inequality_Phi
void SetInitialTrajectory(const std::vector< Eigen::VectorXd > &q_init_in)
Sets the initial trajectory/seed.
Eigen::VectorXd GetEquality() const
Returns the equality constraint values for the entire trajectory.
virtual void PreUpdate()
Updates internal variables before solving, e.g., after setting new values for Rho.
Eigen::RowVectorXd GetScalarTaskJacobian(int t) const
Returns the Jacobian of the scalar task cost at timestep t.
double tau_
Time step duration.
std::vector< std::shared_ptr< KinematicResponse > > kinematic_solutions_
int get_active_nonlinear_equality_constraints_dimension() const
Returns the dimension of the active equality constraints.
Eigen::VectorXd GetJointVelocityConstraint() const
Returns the joint velocity constraint inequality terms (linear).
Eigen::VectorXd xdiff_max_
Maximum change in the variables in a single timestep tau_. Gets set/updated via SetJointVelocityLimit...
Eigen::RowVectorXd GetCostJacobian() const
Returns the Jacobian of the scalar cost over the entire trajectory (Jacobian of GetCost).
Eigen::MatrixXd GetBounds() const
Returns the joint bounds (first column lower, second column upper).
Eigen::SparseMatrix< double > GetInequalityJacobian() const
Returns the sparse Jacobian matrix of the inequality constraints over the entire trajectory.
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).
std::vector< TaskSpaceVector > Phi
int active_nonlinear_equality_constraints_dimension_
double ct
Normalisation of scalar cost and Jacobian over trajectory length.
Eigen::VectorXd q_dot_max_
Joint velocity limit (rad/s)
void SetTau(const double tau_in)
Sets the time discretization tau for the trajectory.
Eigen::SparseMatrix< double > GetEqualityJacobian() const
Returns the sparse Jacobian matrix of the equality constraints over the entire trajectory.
std::vector< Eigen::VectorXd > initial_trajectory_