42 #include <Eigen/Geometry>
46 #include <stomp/utils.h>
53 static const FilterFn NO_FILTER = [](
const Eigen::MatrixXd& , Eigen::MatrixXd& ) {
69 Eigen::MatrixXd smoothing_matrix;
70 stomp::generateSmoothingMatrix(num_timesteps, 1.0 , smoothing_matrix);
71 return [=](
const Eigen::MatrixXd& , Eigen::MatrixXd& filtered_values) {
72 for (
auto row : filtered_values.rowwise())
74 row.transpose() = smoothing_matrix * row.transpose();
88 return [=](
const Eigen::MatrixXd& values, Eigen::MatrixXd& filtered_values) {
89 filtered_values = values;
91 for (
size_t i = 0; i < joints.size(); ++i)
93 for (
int j = 0; j < filtered_values.cols(); ++j)
95 joints.at(i)->enforcePositionBounds(&filtered_values.coeffRef(i, j));
110 return [=](
const Eigen::MatrixXd& values, Eigen::MatrixXd& filtered_values) {
111 Eigen::MatrixXd values_in = values;
112 for (
const auto& filter_fn : filter_functions)
114 filter_fn(values_in, filtered_values);
115 values_in = filtered_values;
const std::vector< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints.
FilterFn chain(const std::vector< FilterFn > &filter_functions)
FilterFn enforcePositionBounds(const moveit::core::JointModelGroup *group)
FilterFn simpleSmoothingMatrix(size_t num_timesteps)
std::function< bool(const Eigen::MatrixXd &values, Eigen::MatrixXd &filtered_values)> FilterFn
A STOMP task definition that allows injecting custom functions for planning.