33 #include <exotica_core/task_initializer.h> 39 BoundedEndPoseProblem::BoundedEndPoseProblem()
44 BoundedEndPoseProblem::~BoundedEndPoseProblem() =
default;
46 Eigen::MatrixXd BoundedEndPoseProblem::GetBounds()
const 48 return scene_->GetKinematicTree().GetJointLimits();
51 void BoundedEndPoseProblem::Instantiate(
const BoundedEndPoseProblemInitializer& init)
53 num_tasks = tasks_.size();
56 for (
int i = 0; i < num_tasks; ++i)
59 length_Phi += tasks_[i]->length;
60 length_jacobian += tasks_[i]->length_jacobian;
62 Phi.SetZero(length_Phi);
63 W = Eigen::MatrixXd::Identity(N, N);
64 if (init.W.rows() > 0)
66 if (init.W.rows() == N)
68 W.diagonal() = init.W;
72 ThrowNamed(
"W dimension mismatch! Expected " << N <<
", got " << init.W.rows());
75 if (flags_ &
KIN_J) jacobian = Eigen::MatrixXd(length_jacobian, N);
76 if (flags_ &
KIN_H) hessian.setConstant(length_jacobian, Eigen::MatrixXd::Zero(N, N));
78 if (init.LowerBound.rows() == N)
80 scene_->GetKinematicTree().SetJointLimitsLower(init.LowerBound);
82 else if (init.LowerBound.rows() != 0)
84 ThrowNamed(
"Lower bound size incorrect! Expected " << N <<
" got " << init.LowerBound.rows());
86 if (init.UpperBound.rows() == N)
88 scene_->GetKinematicTree().SetJointLimitsUpper(init.UpperBound);
90 else if (init.UpperBound.rows() != 0)
92 ThrowNamed(
"Lower bound size incorrect! Expected " << N <<
" got " << init.UpperBound.rows());
96 cost.Initialize(init.Cost, shared_from_this(), dummy);
97 ApplyStartState(
false);
101 void BoundedEndPoseProblem::PreUpdate()
103 PlanningProblem::PreUpdate();
104 for (
int i = 0; i < tasks_.size(); ++i) tasks_[i]->is_used =
false;
108 double BoundedEndPoseProblem::GetScalarCost()
const 110 return cost.ydiff.transpose() * cost.S * cost.ydiff;
113 Eigen::RowVectorXd BoundedEndPoseProblem::GetScalarJacobian()
const 115 return cost.jacobian.transpose() * cost.S * cost.ydiff * 2.0;
118 double BoundedEndPoseProblem::GetScalarTaskCost(
const std::string& task_name)
const 120 for (
int i = 0; i < cost.indexing.size(); ++i)
122 if (cost.tasks[i]->GetObjectName() == task_name)
124 return cost.ydiff.segment(cost.indexing[i].start, cost.indexing[i].length).transpose() * cost.rho(cost.indexing[i].id) * cost.ydiff.segment(cost.indexing[i].start, cost.indexing[i].length);
127 ThrowPretty(
"Cannot get scalar task cost. Task map '" << task_name <<
"' does not exist.");
132 scene_->Update(x, t_start);
133 Phi.SetZero(length_Phi);
134 if (flags_ &
KIN_J) jacobian.setZero();
136 for (
int i = 0; i < length_jacobian; ++i) hessian(i).setZero();
137 for (
int i = 0; i < tasks_.size(); ++i)
139 if (tasks_[i]->is_used)
144 Phi.data.segment(tasks_[i]->start, tasks_[i]->length),
145 jacobian.middleRows(tasks_[i]->start_jacobian, tasks_[i]->length_jacobian),
146 hessian.segment(tasks_[i]->start_jacobian, tasks_[i]->length_jacobian));
148 else if (flags_ & KIN_J)
151 Phi.data.segment(tasks_[i]->start, tasks_[i]->length),
152 Eigen::MatrixXdRef(jacobian.middleRows(tasks_[i]->start_jacobian, tasks_[i]->length_jacobian))
157 tasks_[i]->Update(x, Phi.data.segment(tasks_[i]->start, tasks_[i]->length));
163 cost.Update(Phi, jacobian, hessian);
165 else if (flags_ & KIN_J)
167 cost.Update(Phi, jacobian);
173 ++number_of_problem_updates_;
178 for (
int i = 0; i < cost.indexing.size(); ++i)
180 if (cost.tasks[i]->GetObjectName() == task_name)
182 if (goal.rows() != cost.indexing[i].length)
ThrowPretty(
"Expected length of " << cost.indexing[i].length <<
" and got " << goal.rows());
183 cost.y.data.segment(cost.indexing[i].start, cost.indexing[i].length) = goal;
187 ThrowPretty(
"Cannot set Goal. Task map '" << task_name <<
"' does not exist.");
190 void BoundedEndPoseProblem::SetRho(
const std::string& task_name,
const double& rho)
192 for (
int i = 0; i < cost.indexing.size(); ++i)
194 if (cost.tasks[i]->GetObjectName() == task_name)
196 cost.rho(cost.indexing[i].id) = rho;
201 ThrowPretty(
"Cannot set rho. Task map '" << task_name <<
"' does not exist.");
204 Eigen::VectorXd BoundedEndPoseProblem::GetGoal(
const std::string& task_name)
206 for (
int i = 0; i < cost.indexing.size(); ++i)
208 if (cost.tasks[i]->GetObjectName() == task_name)
210 return cost.y.data.segment(cost.indexing[i].start, cost.indexing[i].length);
213 ThrowPretty(
"Cannot get Goal. Task map '" << task_name <<
"' does not exist.");
216 double BoundedEndPoseProblem::GetRho(
const std::string& task_name)
218 for (
int i = 0; i < cost.indexing.size(); ++i)
220 if (cost.tasks[i]->GetObjectName() == task_name)
222 return cost.rho(cost.indexing[i].id);
225 ThrowPretty(
"Cannot get rho. Task map '" << task_name <<
"' does not exist.");
228 bool BoundedEndPoseProblem::IsValid()
230 Eigen::VectorXd
x = scene_->GetKinematicTree().GetControlledState();
231 Eigen::MatrixXd bounds = scene_->GetKinematicTree().GetJointLimits();
233 std::cout.precision(4);
234 constexpr
double tolerance = 1.e-3;
236 bool succeeded =
true;
237 for (
unsigned int i = 0; i < N; ++i)
239 if (x(i) < bounds(i, 0) - tolerance || x(i) > bounds(i, 1) + tolerance)
241 if (debug_)
HIGHLIGHT_NAMED(
"BoundedEndPoseProblem::IsValid",
"Out of bounds (joint #" << i <<
"): " << bounds(i, 0) <<
" < " << x(i) <<
" < " << bounds(i, 1));
Bound constrained end-pose problem implementation.
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
void AppendVector(std::vector< Val > &orig, const std::vector< Val > &extra)
#define REGISTER_PROBLEM_TYPE(TYPE, DERIV)