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());
78 ThrowNamed(
"Invalid goal time t_goal= " << t_goal_ <<
", where t_start=" <<
t_start);
80 if (init.JointVelocityLimits.size() ==
N)
84 else if (init.JointVelocityLimits.size() == 1)
86 vel_limits = init.JointVelocityLimits(0) * Eigen::VectorXd::Ones(
N);
90 ThrowNamed(
"Dimension mismatch: problem N=" <<
N <<
", but joint velocity limits has dimension " <<
vel_limits.rows());
111 if (
scene_->GetKinematicTree().GetControlledBaseType() !=
exotica::BaseType::FIXED && init.FloatingBaseLowerLimits.rows() > 0 && init.FloatingBaseUpperLimits.rows() > 0)
113 if (
scene_->GetKinematicTree().GetControlledBaseType() ==
exotica::BaseType::FLOATING && init.FloatingBaseLowerLimits.rows() == 6 && init.FloatingBaseUpperLimits.rows() == 6)
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()));
119 else if (
scene_->GetKinematicTree().GetControlledBaseType() ==
exotica::BaseType::PLANAR && init.FloatingBaseLowerLimits.rows() == 3 && init.FloatingBaseUpperLimits.rows() == 3)
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;
std::vector< TaskVectorEntry > map
KinematicRequestFlags flags_
double GetGoalTime() const
TimeIndexedSamplingProblem()
virtual Eigen::VectorXd ApplyStartState(bool update_traj=true)
unsigned int number_of_problem_updates_
TimeIndexedSamplingProblemInitializer parameters
void PreUpdate() override
Eigen::VectorXd GetGoalNEQ(const std::string &task_name)
void SetZero(const int n)
void Update(Eigen::VectorXdRefConst x, const double &t)
void SetGoalEQ(const std::string &task_name, Eigen::VectorXdRefConst goal)
Eigen::VectorXd goal_
Goal state to reach (spatial) at temporal goal (t_goal_)
double GetRhoNEQ(const std::string &task_name)
Eigen::VectorXd vel_limits
void SetRhoEQ(const std::string &task_name, const double &rho)
virtual bool IsValid()
Evaluates whether the problem is valid.
void SetGoalState(Eigen::VectorXdRefConst qT)
virtual void Initialize(const std::vector< exotica::Initializer > &inits, std::shared_ptr< PlanningProblem > prob, TaskSpaceVector &Phi)
void SetRhoNEQ(const std::string &task_name, const double &rho)
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Convenience wrapper for storing references to sub-matrices/vectors.
Eigen::VectorXd GetGoalEQ(const std::string &task_name)
Eigen::VectorXd GetGoalState() const
double GetRhoEQ(const std::string &task_name)
#define HIGHLIGHT_NAMED(name, x)
void SetGoalNEQ(const std::string &task_name, Eigen::VectorXdRefConst goal)
virtual ~TimeIndexedSamplingProblem()
std::vector< double > GetBounds()
std::vector< TaskIndexing > indexing
double t_goal_
Goal time: The time at which goal_ should be reached and the upper bound for the time-dimension.
TaskSpaceVector constraint_phi
void AppendVector(std::vector< Val > &orig, const std::vector< Val > &extra)
void Instantiate(const TimeIndexedSamplingProblemInitializer &init) override
#define REGISTER_PROBLEM_TYPE(TYPE, DERIV)
void SetGoalTime(const double &t)
void Update(const TaskSpaceVector &big_Phi)