chomp_utils.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Mrinal Kalakrishnan */
36 
37 #ifndef CHOMP_UTILS_H_
38 #define CHOMP_UTILS_H_
39 
40 #include <iostream>
41 #include <Eigen/Core>
43 
44 namespace chomp
45 {
46 static const int DIFF_RULE_LENGTH = 7;
47 
48 // the differentiation rules (centered at the center)
49 static const double DIFF_RULES[3][DIFF_RULE_LENGTH] = {
50  { 0, 0, -2 / 6.0, -3 / 6.0, 6 / 6.0, -1 / 6.0, 0 }, // velocity
51  { 0, -1 / 12.0, 16 / 12.0, -30 / 12.0, 16 / 12.0, -1 / 12.0, 0 }, // acceleration
52  { 0, 1 / 12.0, -17 / 12.0, 46 / 12.0, -46 / 12.0, 17 / 12.0, -1 / 12.0 } // jerk
53 };
54 
55 static inline void jointStateToArray(const moveit::core::RobotModelConstPtr& kmodel,
56  const sensor_msgs::JointState& joint_state, const std::string& planning_group_name,
57  Eigen::MatrixXd::RowXpr joint_array)
58 {
59  const moveit::core::JointModelGroup* group = kmodel->getJointModelGroup(planning_group_name);
60  std::vector<const moveit::core::JointModel*> models = group->getActiveJointModels();
61 
62  for (unsigned int i = 0; i < joint_state.position.size(); i++)
63  {
64  for (size_t j = 0; j < models.size(); j++)
65  {
66  if (models[j]->getName() == joint_state.name[i])
67  {
68  joint_array(0, j) = joint_state.position[i];
69  }
70  }
71  }
72 }
73 
74 // copied from geometry/angles/angles.h
75 static inline double normalizeAnglePositive(double angle)
76 {
77  return fmod(fmod(angle, 2.0 * M_PI) + 2.0 * M_PI, 2.0 * M_PI);
78 }
79 
80 static inline double normalizeAngle(double angle)
81 {
82  double a = normalizeAnglePositive(angle);
83  if (a > M_PI)
84  a -= 2.0 * M_PI;
85  return a;
86 }
87 
88 static inline double shortestAngularDistance(double start, double end)
89 {
91  if (res > M_PI)
92  {
93  res = -(2.0 * M_PI - res);
94  }
95  return normalizeAngle(res);
96 }
97 
98 } // namespace chomp
99 
100 #endif /* CHOMP_UTILS_H_ */
static const int DIFF_RULE_LENGTH
Definition: chomp_utils.h:46
std::string getName(void *handle)
#define M_PI
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)
Definition: chomp_utils.h:55
static const double DIFF_RULES[3][DIFF_RULE_LENGTH]
Definition: chomp_utils.h:49
static double shortestAngularDistance(double start, double end)
Definition: chomp_utils.h:88
const std::vector< const JointModel * > & getActiveJointModels() const
static double normalizeAngle(double angle)
Definition: chomp_utils.h:80
static double normalizeAnglePositive(double angle)
Definition: chomp_utils.h:75


chomp_motion_planner
Author(s): Gil Jones , Mrinal Kalakrishnan
autogenerated on Wed Jul 10 2019 04:03:20