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()
std::vector< Eigen::Triplet< double > > GetEqualityJacobianTriplets() const
Returns a vector of triplets to fill a sparse Jacobian for the equality constraints.
double GetDuration() const
Returns the duration of the trajectory (T * tau).
void Update(Eigen::VectorXdRefConst x_trajectory_in)
Updates the entire problem from a given trajectory (e.g., used in an optimization solver) ...
std::vector< std::pair< int, int > > active_nonlinear_equality_constraints_
Eigen::MatrixXd GetJointVelocityConstraintBounds() const
Returns the joint velocity constraint bounds (constant terms).
double GetScalarTaskCost(int t) const
Returns the scalar task cost at timestep t.
void SetT(const int T_in)
Sets the number of timesteps in the trajectory. Note: Rho/Goal need to be updated for every timestep ...
Eigen::VectorXd GetInequality() const
Returns the inequality constraint values for the entire trajectory.
TimeIndexedTask equality
General equality task.
Eigen::RowVectorXd GetScalarTaskJacobian(int t) const
Returns the Jacobian of the scalar task cost at timestep t.
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).
double get_ct() const
Returns the cost scaling factor.
Eigen::VectorXd GetJointVelocityConstraint() const
Returns the joint velocity constraint inequality terms (linear).
int joint_velocity_constraint_dimension_
double GetRho(const std::string &task_name, int t=0)
Returns the precision (Rho) for a given task at a given timestep (cost task).
std::vector< Eigen::VectorXd > xdiff
std::vector< Eigen::Triplet< double > > GetJointVelocityConstraintJacobianTriplets() const
Returns the joint velocity constraint Jacobian as triplets.
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_
virtual void ReinitializeVariables()
int get_active_nonlinear_inequality_constraints_dimension() const
Returns the dimension of the active inequality constraints.
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_
int GetT() const
Returns the number of timesteps in the trajectory.
double GetTau() const
Returns the time discretization tau for the trajectory.
double GetCost() const
Returns the scalar cost for the entire trajectory (both task and transition cost).
TimeIndexedTask inequality
General inequality task.
Eigen::VectorXd GetJointVelocityLimits() const
Returns the per-DoF joint velocity limit vector.
std::vector< Hessian > hessian
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.
Eigen::RowVectorXd GetScalarTransitionJacobian(int t) const
Returns the Jacobian of the transition cost at timestep t.
std::vector< Eigen::Triplet< double > > GetInequalityJacobianTriplets() const
Returns a vector of triplets to fill a sparse Jacobian for the inequality constraints.
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).
double w_scale_
Kinematic system transition error covariance multiplier (constant throughout the trajectory) ...
virtual ~AbstractTimeIndexedProblem()
int get_joint_velocity_constraint_dimension() const
Returns the dimension of the joint velocity constraint (linear inequality).
double GetScalarTransitionCost(int t) const
Returns the scalar transition cost (x^T*W*x) at timestep t.
Eigen::RowVectorXd GetCostJacobian() const
Returns the Jacobian of the scalar cost over the entire trajectory (Jacobian of GetCost).
Eigen::VectorXd GetGoal(const std::string &task_name, int t=0)
Returns the goal for a given task at a given timestep (cost task).
Eigen::MatrixXd GetBounds() const
Returns the joint bounds (first column lower, second column upper).
int T_
Number of time steps.
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.
std::vector< Eigen::MatrixXd > jacobian
std::vector< Eigen::VectorXd > GetInitialTrajectory() const
Returns the initial trajectory/seed.
TaskSpaceVector inequality_Phi
void SetInitialTrajectory(const std::vector< Eigen::VectorXd > &q_init_in)
Sets the initial trajectory/seed.
void ValidateTimeIndex(int &t_in) const
Checks the desired time index for bounds and supports -1 indexing.
virtual void PreUpdate()
Updates internal variables before solving, e.g., after setting new values for Rho.
double tau_
Time step duration.
std::vector< std::shared_ptr< KinematicResponse > > kinematic_solutions_
Eigen::SparseMatrix< double > GetEqualityJacobian() const
Returns the sparse Jacobian matrix of the equality constraints over the entire trajectory.
Eigen::VectorXd xdiff_max_
Maximum change in the variables in a single timestep tau_. Gets set/updated via SetJointVelocityLimit...
int get_active_nonlinear_equality_constraints_dimension() const
Returns the dimension of the active equality constraints.
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).
Eigen::VectorXd GetEquality() const
Returns the equality constraint values for the entire trajectory.
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.
std::vector< Eigen::VectorXd > initial_trajectory_
Eigen::SparseMatrix< double > GetInequalityJacobian() const
Returns the sparse Jacobian matrix of the inequality constraints over the entire trajectory.