42 #include <Eigen/Geometry>
53 using StateValidatorFn = std::function<double(
const Eigen::VectorXd& state_positions)>;
78 CostFn cost_fn = [=](
const Eigen::MatrixXd& values, Eigen::VectorXd& costs,
bool& validity) {
80 costs.setZero(values.cols());
89 std::vector<std::pair<long, long>> invalid_windows;
90 bool in_invalid_window =
false;
91 for (
int timestep = 0; timestep < values.cols(); ++timestep)
97 Eigen::VectorXd current = values.col(timestep);
98 costs(timestep) += state_validator_fn(current);
99 bool found_invalid_state = costs(timestep) > 0.0;
102 bool continue_interpolation =
103 !found_invalid_state && timestep < (values.cols() - 1) && interpolation_step_size > 0.0;
104 if (continue_interpolation)
106 Eigen::VectorXd next = values.col(timestep + 1);
108 const double interpolation_step = std::min(0.5, interpolation_step_size / (next - current).norm());
109 for (
double interpolation_fraction = interpolation_step; interpolation_fraction < 1.0;
110 interpolation_fraction += interpolation_step)
112 Eigen::VectorXd sample_vec = (1 - interpolation_fraction) * current + interpolation_fraction * next;
114 double penalty = state_validator_fn(sample_vec);
115 found_invalid_state = penalty > 0.0;
116 if (found_invalid_state)
119 costs(timestep) += (1 - interpolation_fraction) * penalty;
120 costs(timestep + 1) += interpolation_fraction * penalty;
127 if (found_invalid_state)
133 if (!in_invalid_window)
136 invalid_windows.emplace_back(timestep, timestep);
137 in_invalid_window =
true;
141 invalid_windows.back().second = timestep;
146 in_invalid_window =
false;
153 for (
const auto& [start, end] : invalid_windows)
157 const double window_cost = costs(Eigen::seq(start, end)).sum();
161 const double window_size =
static_cast<double>(end - start) + 1;
162 const double sigma = std::max(1.0, 0.5 * window_size);
163 const double mu = 0.5 * (start + end);
168 const long kernel_start = mu -
static_cast<long>(sigma) * 4;
169 const long kernel_end = mu +
static_cast<long>(sigma) * 4;
170 const long bounded_kernel_start = std::max(0l, kernel_start);
171 const long bounded_kernel_end = std::min(values.cols() - 1, kernel_end);
172 for (
auto j = bounded_kernel_start; j <= bounded_kernel_end; ++j)
174 costs(j) = std::exp(-std::pow(j - mu, 2) / (2 * std::pow(sigma, 2))) / (sigma * std::sqrt(2 * M_PI));
178 const double cost_sum = costs(Eigen::seq(bounded_kernel_start, bounded_kernel_end)).sum();
179 costs(Eigen::seq(bounded_kernel_start, bounded_kernel_end)) *= window_cost / cost_sum;
203 const auto& group_name = group ? group->
getName() :
"";
205 StateValidatorFn collision_validator_fn = [=](
const Eigen::VectorXd& positions) {
212 return planning_scene->isStateColliding(state, group_name) ? collision_penalty : 0.0;
232 const moveit_msgs::msg::Constraints& constraints_msg,
double cost_scale)
239 StateValidatorFn constraints_validator_fn = [=](
const Eigen::VectorXd& positions) {
261 return [=](
const Eigen::MatrixXd& values, Eigen::VectorXd& overall_costs,
bool& overall_validity) {
262 overall_validity =
true;
263 overall_costs.setZero(values.cols());
265 auto costs = overall_costs;
266 for (
const auto& cost_fn : cost_functions)
269 cost_fn(values, costs, valid);
272 overall_validity = overall_validity && valid;
273 overall_costs += costs;
A class that contains many different constraints, and can check RobotState *versus the full set....
bool add(const moveit_msgs::msg::Constraints &c, const moveit::core::Transforms &tf)
Add all known constraints.
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const
Determines whether all constraints are satisfied by state, returning a single evaluation result.
const std::vector< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints.
const std::string & getName() const
Get the name of the joint group.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
void update(bool force=false)
Update all transforms.
Helper functions for converting between MoveIt types and plain Eigen types.
This namespace includes the central class for representing planning contexts.
CostFn getCostFunctionFromStateValidator(const StateValidatorFn &state_validator_fn, double interpolation_step_size)
CostFn getConstraintsCostFunction(const std::shared_ptr< const planning_scene::PlanningScene > &planning_scene, const moveit::core::JointModelGroup *group, const moveit_msgs::msg::Constraints &constraints_msg, double cost_scale)
constexpr double COL_CHECK_DISTANCE
CostFn sum(const std::vector< CostFn > &cost_functions)
constexpr double CONSTRAINT_CHECK_DISTANCE
CostFn getCollisionCostFunction(const std::shared_ptr< const planning_scene::PlanningScene > &planning_scene, const moveit::core::JointModelGroup *group, double collision_penalty)
void setJointPositions(const Eigen::VectorXd &values, const Joints &joints, moveit::core::RobotState &state)
std::function< bool(const Eigen::MatrixXd &values, Eigen::VectorXd &costs, bool &validity)> CostFn
std::function< double(const Eigen::VectorXd &state_positions)> StateValidatorFn
A STOMP task definition that allows injecting custom functions for planning.
double distance
The distance evaluation from the constraint or constraints.