41 if (init.LowerBound.rows() ==
N)
43 scene_->GetKinematicTree().SetJointLimitsLower(init.LowerBound);
45 else if (init.LowerBound.rows() != 0)
47 ThrowNamed(
"Lower bound size incorrect! Expected " <<
N <<
" got " << init.LowerBound.rows());
49 if (init.UpperBound.rows() ==
N)
51 scene_->GetKinematicTree().SetJointLimitsUpper(init.UpperBound);
53 else if (init.UpperBound.rows() != 0)
55 ThrowNamed(
"Lower bound size incorrect! Expected " <<
N <<
" got " << init.UpperBound.rows());
69 for (
int i = 0; i <
tasks_.size(); ++i)
tasks_[i]->is_used =
false;
77 for (
int i = 0; i < T_; ++i) kinematic_solutions_[i] = std::make_shared<KinematicResponse>(*
scene_->GetKinematicTree().GetKinematicResponse());
102 scene_->Update(x_in, static_cast<double>(t) *
tau_);
146 if (t > 0)
xdiff[t] =
x[t] -
x[t - 1];
167 Phi.assign(
T_, y_ref_);
169 x.assign(
T_, Eigen::VectorXd::Zero(
N));
170 xdiff.assign(
T_, Eigen::VectorXd::Zero(
N));
193 bool succeeded =
true;
194 auto bounds =
scene_->GetKinematicTree().GetJointLimits();
196 std::cout.precision(4);
199 for (
int t = 0;
t <
T_; ++
t)
204 for (
int i = 0; i <
N; ++i)
206 constexpr
double tolerance = 1.e-3;
207 if (
x[
t](i) < bounds(i, 0) - tolerance ||
x[
t](i) > bounds(i, 1) + tolerance)
209 if (
debug_)
HIGHLIGHT_NAMED(
"TimeIndexedProblem::IsValid",
"State at timestep " <<
t <<
" is out of bounds: joint #" << i <<
": " << bounds(i, 0) <<
" < " <<
x[
t](i) <<
" < " << bounds(i, 1));
std::vector< TaskVectorEntry > map
KinematicRequestFlags flags_
virtual Eigen::VectorXd ApplyStartState(bool update_traj=true)
unsigned int number_of_problem_updates_
bool IsValid() override
Evaluates whether the problem is valid.
void SetZero(const int n)
std::vector< Eigen::VectorXd > xdiff
geometry_msgs::TransformStamped t
void ReinitializeVariables() override
void Update(Eigen::VectorXdRefConst x, int t) override
Updates an individual timestep from a given state vector.
virtual void Initialize(const std::vector< exotica::Initializer > &inits, std::shared_ptr< PlanningProblem > prob, TaskSpaceVector &Phi)
void Update(const TaskSpaceVector &big_Phi, Eigen::MatrixXdRefConst big_dPhi_dx, Eigen::MatrixXdRefConst big_dPhi_du, HessianRefConst big_ddPhi_ddx, HessianRefConst big_ddPhi_ddu, HessianRefConst big_ddPhi_dxdu, int t)
std::vector< Hessian > hessian
Eigen::Array< Eigen::MatrixXd, Eigen::Dynamic, 1 > Hessian
std::vector< Eigen::VectorXd > x
Current internal problem state.
void ValidateTimeIndex(int &t_in) const
Checks the desired time index for bounds and supports -1 indexing.
void PreUpdate() override
Updates internal variables before solving, e.g., after setting new values for Rho.
int T_
Number of time steps.
BoundedTimeIndexedProblemInitializer parameters_
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Convenience wrapper for storing references to sub-matrices/vectors.
void UpdateMultipleTaskKinematics(std::vector< std::shared_ptr< KinematicResponse >> responses)
TimeIndexedTask cost
Cost task.
void Instantiate(const BoundedTimeIndexedProblemInitializer &init) override
Instantiates the problem from an Initializer.
std::vector< Eigen::MatrixXd > jacobian
void ReinitializeVariables(int _T, std::shared_ptr< PlanningProblem > _prob, const TaskSpaceVector &_Phi)
#define HIGHLIGHT_NAMED(name, x)
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
double tau_
Time step duration.
std::vector< std::shared_ptr< KinematicResponse > > kinematic_solutions_
Bound constrained time-indexed problem.
Eigen::VectorXd xdiff_max_
Maximum change in the variables in a single timestep tau_. Gets set/updated via SetJointVelocityLimit...
void AppendVector(std::vector< Val > &orig, const std::vector< Val > &extra)
std::vector< TaskSpaceVector > Phi
#define REGISTER_PROBLEM_TYPE(TYPE, DERIV)
double ct
Normalisation of scalar cost and Jacobian over trajectory length.
Eigen::VectorXd q_dot_max_
Joint velocity limit (rad/s)
std::vector< Eigen::VectorXd > initial_trajectory_