46 std::vector<double> bounds;
47 auto joint_limits =
scene_->GetKinematicTree().GetJointLimits();
50 for (
unsigned int i = 0; i <
N; ++i)
52 bounds[i] = joint_limits(i, 0);
53 bounds[i +
N] = joint_limits(i, 1);
63 if (
init.Goal.size() ==
N)
67 else if (
init.Goal.size() == 0)
69 goal_ = Eigen::VectorXd::Zero(
N);
73 ThrowNamed(
"Dimension mismatch: problem N=" <<
N <<
", but goal state has dimension " <<
goal_.rows());
80 if (
init.JointVelocityLimits.size() ==
N)
84 else if (
init.JointVelocityLimits.size() == 1)
90 ThrowNamed(
"Dimension mismatch: problem N=" <<
N <<
", but joint velocity limits has dimension " <<
vel_limits.rows());
115 scene_->GetKinematicTree().SetFloatingBaseLimitsPosXYZEulerZYX(
116 std::vector<double>(
init.FloatingBaseLowerLimits.data(),
init.FloatingBaseLowerLimits.data() +
init.FloatingBaseLowerLimits.size()),
117 std::vector<double>(
init.FloatingBaseUpperLimits.data(),
init.FloatingBaseUpperLimits.data() +
init.FloatingBaseUpperLimits.size()));
121 scene_->GetKinematicTree().SetPlanarBaseLimitsPosXYEulerZ(
122 std::vector<double>(
init.FloatingBaseLowerLimits.data(),
init.FloatingBaseLowerLimits.data() +
init.FloatingBaseLowerLimits.size()),
123 std::vector<double>(
init.FloatingBaseUpperLimits.data(),
init.FloatingBaseUpperLimits.data() +
init.FloatingBaseUpperLimits.size()));
152 ThrowPretty(
"Dimensionality of goal state wrong: Got " << qT.rows() <<
", expected " <<
N);
167 ThrowPretty(
"Cannot set Goal. Task map '" << task_name <<
"' does not exist.");
181 ThrowPretty(
"Cannot set rho. Task map '" << task_name <<
"' does not exist.");
193 ThrowPretty(
"Cannot get Goal. Task map '" << task_name <<
"' does not exist.");
205 ThrowPretty(
"Cannot get rho. Task map '" << task_name <<
"' does not exist.");
219 ThrowPretty(
"Cannot set Goal. Task map '" << task_name <<
"' does not exist.");
233 ThrowPretty(
"Cannot set rho. Task map '" << task_name <<
"' does not exist.");
245 ThrowPretty(
"Cannot get Goal. Task map '" << task_name <<
"' does not exist.");
257 ThrowPretty(
"Cannot get rho. Task map '" << task_name <<
"' does not exist.");
277 HIGHLIGHT_NAMED(
"TimeIndexedSamplingProblem::IsValid",
"Equality valid? = " << equality_is_valid <<
"\tInequality valid? = " << inequality_is_valid);
280 return (inequality_is_valid && equality_is_valid);
286 for (
int i = 0; i <
tasks_.size(); ++i)
tasks_[i]->is_used =
false;