37 void UnconstrainedTimeIndexedProblem::Instantiate(
const UnconstrainedTimeIndexedProblemInitializer& init)
39 this->parameters_ = init;
41 N = scene_->GetKinematicTree().GetNumControlledJoints();
43 w_scale_ = this->parameters_.Wrate;
44 W = Eigen::MatrixXd::Identity(N, N) * w_scale_;
45 if (this->parameters_.W.rows() > 0)
47 if (this->parameters_.W.rows() == N)
49 W.diagonal() = this->parameters_.W * w_scale_;
53 ThrowNamed(
"W dimension mismatch! Expected " << N <<
", got " << this->parameters_.W.rows());
57 cost.Initialize(this->parameters_.Cost, shared_from_this(), cost_Phi);
59 T_ = this->parameters_.T;
60 tau_ = this->parameters_.tau;
61 ApplyStartState(
false);
62 ReinitializeVariables();
65 void UnconstrainedTimeIndexedProblem::ReinitializeVariables()
67 if (debug_)
HIGHLIGHT_NAMED(
"UnconstrainedTimeIndexedProblem",
"Initialize problem with T=" << T_);
69 num_tasks = tasks_.size();
73 for (
int i = 0; i < num_tasks; ++i)
76 length_Phi += tasks_[i]->length;
77 length_jacobian += tasks_[i]->length_jacobian;
81 Phi.assign(T_, y_ref_);
83 x.assign(T_, Eigen::VectorXd::Zero(N));
84 xdiff.assign(T_, Eigen::VectorXd::Zero(N));
85 if (flags_ &
KIN_J) jacobian.assign(T_, Eigen::MatrixXd(length_jacobian, N));
89 Htmp.setConstant(length_jacobian, Eigen::MatrixXd::Zero(N, N));
90 hessian.assign(T_, Htmp);
94 initial_trajectory_.resize(T_, scene_->GetControlledState());
96 cost.ReinitializeVariables(T_, shared_from_this(), cost_Phi);
100 xdiff_max_ = q_dot_max_ * tau_;
105 void UnconstrainedTimeIndexedProblem::PreUpdate()
107 PlanningProblem::PreUpdate();
108 for (
int i = 0; i < tasks_.size(); ++i) tasks_[i]->is_used =
false;
114 kinematic_solutions_.clear();
115 kinematic_solutions_.resize(T_);
116 for (
int i = 0; i < T_; ++i) kinematic_solutions_[i] = std::make_shared<KinematicResponse>(*scene_->GetKinematicTree().GetKinematicResponse());
121 ValidateTimeIndex(t);
127 scene_->GetKinematicTree().SetKinematicResponse(kinematic_solutions_[t]);
132 std::vector<std::shared_ptr<KinematicResponse>> kinematics_solutions{kinematic_solutions_[t]};
136 kinematics_solutions.emplace_back((t == 0) ? kinematic_solutions_[t] : kinematic_solutions_[t - 1]);
139 PlanningProblem::UpdateMultipleTaskKinematics(kinematics_solutions);
141 scene_->Update(x_in, static_cast<double>(t) * tau_);
143 Phi[t].SetZero(length_Phi);
144 if (flags_ &
KIN_J) jacobian[t].setZero();
146 for (
int i = 0; i < length_jacobian; ++i) hessian[t](i).setZero();
147 for (
int i = 0; i < num_tasks; ++i)
150 if (tasks_[i]->is_used)
154 tasks_[i]->Update(
x[t],
155 Phi[t].data.segment(tasks_[i]->start, tasks_[i]->length),
156 jacobian[t].middleRows(tasks_[i]->start_jacobian, tasks_[i]->length_jacobian),
157 hessian[t].segment(tasks_[i]->start_jacobian, tasks_[i]->length_jacobian));
159 else if (flags_ & KIN_J)
161 tasks_[i]->Update(
x[t],
162 Phi[t].data.segment(tasks_[i]->start, tasks_[i]->length),
163 Eigen::MatrixXdRef(jacobian[t].middleRows(tasks_[i]->start_jacobian, tasks_[i]->length_jacobian))
168 tasks_[i]->Update(
x[t], Phi[t].data.segment(tasks_[i]->start, tasks_[i]->length));
174 cost.Update(Phi[t], jacobian[t], hessian[t], t);
176 else if (flags_ & KIN_J)
178 cost.Update(Phi[t], jacobian[t], t);
182 cost.Update(Phi[t], t);
185 if (t > 0) xdiff[t] =
x[t] -
x[t - 1];
187 ++number_of_problem_updates_;
190 bool UnconstrainedTimeIndexedProblem::IsValid()
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
Unconstrained time-indexed problem.
void AppendVector(std::vector< Val > &orig, const std::vector< Val > &extra)
#define REGISTER_PROBLEM_TYPE(TYPE, DERIV)