37 #ifndef CHOMP_UTILS_H_ 38 #define CHOMP_UTILS_H_ 50 { 0, 0, -2 / 6.0, -3 / 6.0, 6 / 6.0, -1 / 6.0, 0 },
51 { 0, -1 / 12.0, 16 / 12.0, -30 / 12.0, 16 / 12.0, -1 / 12.0, 0 },
52 { 0, 1 / 12.0, -17 / 12.0, 46 / 12.0, -46 / 12.0, 17 / 12.0, -1 / 12.0 }
56 const sensor_msgs::JointState& joint_state,
const std::string& planning_group_name,
57 Eigen::MatrixXd::RowXpr joint_array)
62 for (
unsigned int i = 0; i < joint_state.position.size(); i++)
64 for (
size_t j = 0; j < models.size(); j++)
66 if (models[j]->
getName() == joint_state.name[i])
68 joint_array(0, j) = joint_state.position[i];
77 return fmod(fmod(angle, 2.0 *
M_PI) + 2.0 *
M_PI, 2.0 *
M_PI);
93 res = -(2.0 *
M_PI - res);
static const int DIFF_RULE_LENGTH
std::string getName(void *handle)
static void jointStateToArray(const moveit::core::RobotModelConstPtr &kmodel, const sensor_msgs::JointState &joint_state, const std::string &planning_group_name, Eigen::MatrixXd::RowXpr joint_array)
static const double DIFF_RULES[3][DIFF_RULE_LENGTH]
static double shortestAngularDistance(double start, double end)
const std::vector< const JointModel * > & getActiveJointModels() const
static double normalizeAngle(double angle)
static double normalizeAnglePositive(double angle)