37 UnconstrainedEndPoseProblem::UnconstrainedEndPoseProblem()
42 UnconstrainedEndPoseProblem::~UnconstrainedEndPoseProblem() =
default;
44 void UnconstrainedEndPoseProblem::Instantiate(
const UnconstrainedEndPoseProblemInitializer& init)
46 num_tasks = tasks_.size();
49 for (
int i = 0; i < num_tasks; ++i)
52 length_Phi += tasks_[i]->length;
53 length_jacobian += tasks_[i]->length_jacobian;
55 Phi.SetZero(length_Phi);
56 W = Eigen::MatrixXd::Identity(N, N);
57 if (init.W.rows() > 0)
59 if (init.W.rows() == N)
61 W.diagonal() = init.W;
65 ThrowNamed(
"W dimension mismatch! Expected " << N <<
", got " << init.W.rows());
68 if (flags_ &
KIN_J) jacobian = Eigen::MatrixXd(length_jacobian, N);
69 if (flags_ &
KIN_H) hessian.setConstant(length_jacobian, Eigen::MatrixXd::Zero(N, N));
71 if (init.NominalState.rows() > 0 && init.NominalState.rows() != N)
ThrowNamed(
"Invalid size of NominalState (" << init.NominalState.rows() <<
"), expected: " << N);
72 if (init.NominalState.rows() == N) q_nominal = init.NominalState;
74 cost.Initialize(init.Cost, shared_from_this(), dummy);
75 ApplyStartState(
false);
79 void UnconstrainedEndPoseProblem::PreUpdate()
81 PlanningProblem::PreUpdate();
82 for (
int i = 0; i < tasks_.size(); ++i) tasks_[i]->is_used =
false;
86 double UnconstrainedEndPoseProblem::GetScalarCost()
const 88 return cost.ydiff.transpose() * cost.S * cost.ydiff;
91 Eigen::RowVectorXd UnconstrainedEndPoseProblem::GetScalarJacobian()
const 93 return cost.jacobian.transpose() * cost.S * cost.ydiff * 2.0;
96 double UnconstrainedEndPoseProblem::GetScalarTaskCost(
const std::string& task_name)
const 98 const Eigen::VectorXd ydiff = cost.GetTaskError(task_name);
99 return ydiff.transpose() * GetRho(task_name) * ydiff;
104 scene_->Update(x, t_start);
105 Phi.SetZero(length_Phi);
106 if (flags_ &
KIN_J) jacobian.setZero();
108 for (
int i = 0; i < length_jacobian; ++i) hessian(i).setZero();
109 for (
int i = 0; i < tasks_.size(); ++i)
111 if (tasks_[i]->is_used)
116 Phi.data.segment(tasks_[i]->start, tasks_[i]->length),
117 jacobian.middleRows(tasks_[i]->start_jacobian, tasks_[i]->length_jacobian),
118 hessian.segment(tasks_[i]->start_jacobian, tasks_[i]->length_jacobian));
120 else if (flags_ & KIN_J)
123 Phi.data.segment(tasks_[i]->start, tasks_[i]->length),
124 Eigen::MatrixXdRef(jacobian.middleRows(tasks_[i]->start_jacobian, tasks_[i]->length_jacobian))
129 tasks_[i]->Update(x, Phi.data.segment(tasks_[i]->start, tasks_[i]->length));
135 cost.Update(Phi, jacobian, hessian);
137 else if (flags_ & KIN_J)
139 cost.Update(Phi, jacobian);
145 ++number_of_problem_updates_;
150 for (
int i = 0; i < cost.indexing.size(); ++i)
152 if (cost.tasks[i]->GetObjectName() == task_name)
154 if (goal.rows() != cost.indexing[i].length)
ThrowPretty(
"Expected length of " << cost.indexing[i].length <<
" and got " << goal.rows());
155 cost.y.data.segment(cost.indexing[i].start, cost.indexing[i].length) = goal;
159 ThrowPretty(
"Cannot set Goal. Task map '" << task_name <<
"' does not exist.");
162 void UnconstrainedEndPoseProblem::SetRho(
const std::string& task_name,
const double& rho)
164 for (
int i = 0; i < cost.indexing.size(); ++i)
166 if (cost.tasks[i]->GetObjectName() == task_name)
168 cost.rho(cost.indexing[i].id) = rho;
173 ThrowPretty(
"Cannot set rho. Task map '" << task_name <<
"' does not exist.");
176 Eigen::VectorXd UnconstrainedEndPoseProblem::GetGoal(
const std::string& task_name)
const 178 for (
int i = 0; i < cost.indexing.size(); ++i)
180 if (cost.tasks[i]->GetObjectName() == task_name)
182 return cost.y.data.segment(cost.indexing[i].start, cost.indexing[i].length);
185 ThrowPretty(
"Cannot get Goal. Task map '" << task_name <<
"' does not exist.");
188 double UnconstrainedEndPoseProblem::GetRho(
const std::string& task_name)
const 190 for (
int i = 0; i < cost.indexing.size(); ++i)
192 if (cost.tasks[i]->GetObjectName() == task_name)
194 return cost.rho(cost.indexing[i].id);
197 ThrowPretty(
"Cannot get rho. Task map '" << task_name <<
"' does not exist.");
200 Eigen::VectorXd UnconstrainedEndPoseProblem::GetNominalPose()
const 207 if (qNominal_in.rows() == N)
208 q_nominal = qNominal_in;
210 ThrowPretty(
"Cannot set q_nominal - wrong number of rows (expected " 211 << N <<
", received " << qNominal_in.rows() <<
").");
214 int UnconstrainedEndPoseProblem::GetTaskId(
const std::string& task_name)
const 216 for (
int i = 0; i < cost.indexing.size(); ++i)
218 if (cost.tasks[i]->GetObjectName() == task_name)
223 ThrowPretty(
"Cannot get task. Task map '" << task_name <<
"' does not exist.");
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Convenience wrapper for storing references to sub-matrices/vectors.
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
void AppendVector(std::vector< Val > &orig, const std::vector< Val > &extra)
#define REGISTER_PROBLEM_TYPE(TYPE, DERIV)