37 void BoundedTimeIndexedProblem::Instantiate(
const BoundedTimeIndexedProblemInitializer& init)
39 this->parameters_ = init;
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());
58 cost.Initialize(this->parameters_.Cost, shared_from_this(), cost_Phi);
60 T_ = this->parameters_.T;
61 tau_ = this->parameters_.tau;
62 ApplyStartState(
false);
63 ReinitializeVariables();
66 void BoundedTimeIndexedProblem::PreUpdate()
68 PlanningProblem::PreUpdate();
69 for (
int i = 0; i < tasks_.size(); ++i) tasks_[i]->is_used =
false;
75 kinematic_solutions_.clear();
76 kinematic_solutions_.resize(T_);
77 for (
int i = 0; i < T_; ++i) kinematic_solutions_[i] = std::make_shared<KinematicResponse>(*scene_->GetKinematicTree().GetKinematicResponse());
88 scene_->GetKinematicTree().SetKinematicResponse(kinematic_solutions_[t]);
93 std::vector<std::shared_ptr<KinematicResponse>> kinematics_solutions{kinematic_solutions_[t]};
97 kinematics_solutions.emplace_back((t == 0) ? kinematic_solutions_[t] : kinematic_solutions_[t - 1]);
100 PlanningProblem::UpdateMultipleTaskKinematics(kinematics_solutions);
102 scene_->Update(x_in, static_cast<double>(t) * tau_);
104 Phi[t].SetZero(length_Phi);
105 if (flags_ &
KIN_J) jacobian[t].setZero();
107 for (
int i = 0; i < length_jacobian; ++i) hessian[t](i).setZero();
108 for (
int i = 0; i < num_tasks; ++i)
111 if (tasks_[i]->is_used)
115 tasks_[i]->Update(
x[t],
116 Phi[t].data.segment(tasks_[i]->start, tasks_[i]->length),
117 jacobian[t].middleRows(tasks_[i]->start_jacobian, tasks_[i]->length_jacobian),
118 hessian[t].segment(tasks_[i]->start_jacobian, tasks_[i]->length_jacobian));
120 else if (flags_ & KIN_J)
122 tasks_[i]->Update(
x[t],
123 Phi[t].data.segment(tasks_[i]->start, tasks_[i]->length),
124 Eigen::MatrixXdRef(jacobian[t].middleRows(tasks_[i]->start_jacobian, tasks_[i]->length_jacobian))
129 tasks_[i]->Update(
x[t], Phi[t].data.segment(tasks_[i]->start, tasks_[i]->length));
135 cost.Update(Phi[t], jacobian[t], hessian[t], t);
137 else if (flags_ & KIN_J)
139 cost.Update(Phi[t], jacobian[t], t);
143 cost.Update(Phi[t], t);
146 if (t > 0) xdiff[t] =
x[t] -
x[t - 1];
148 ++number_of_problem_updates_;
151 void BoundedTimeIndexedProblem::ReinitializeVariables()
153 if (debug_)
HIGHLIGHT_NAMED(
"BoundedTimeIndexedProblem",
"Initialize problem with T=" << T_);
155 num_tasks = tasks_.size();
159 for (
int i = 0; i < num_tasks; ++i)
162 length_Phi += tasks_[i]->length;
163 length_jacobian += tasks_[i]->length_jacobian;
167 Phi.assign(T_, y_ref_);
169 x.assign(T_, Eigen::VectorXd::Zero(N));
170 xdiff.assign(T_, Eigen::VectorXd::Zero(N));
171 if (flags_ &
KIN_J) jacobian.assign(T_, Eigen::MatrixXd(length_jacobian, N));
175 Htmp.setConstant(length_jacobian, Eigen::MatrixXd::Zero(N, N));
176 hessian.assign(T_, Htmp);
180 initial_trajectory_.resize(T_, scene_->GetControlledState());
182 cost.ReinitializeVariables(T_, shared_from_this(), cost_Phi);
185 ct = 1.0 / tau_ / T_;
186 xdiff_max_ = q_dot_max_ * tau_;
191 bool BoundedTimeIndexedProblem::IsValid()
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
void SetZero(const int n)
Eigen::Array< Eigen::MatrixXd, Eigen::Dynamic, 1 > Hessian
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Convenience wrapper for storing references to sub-matrices/vectors.
#define HIGHLIGHT_NAMED(name, x)
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
Bound constrained time-indexed problem.
void AppendVector(std::vector< Val > &orig, const std::vector< Val > &extra)
#define REGISTER_PROBLEM_TYPE(TYPE, DERIV)