34 std::vector<double> linear_path, angular_path,
path;
35 std::vector<double> path_matrix[2];
36 geometry_msgs::Pose
pose;
53 linear_path = path_matrix[0];
54 angular_path = path_matrix[1];
56 if (fabs(linear_path.back()) > fabs(angular_path.back()))
64 norm_factor = 1/path.back();
67 for (
unsigned int i = 0; i < linear_path.size(); i++)
69 if (linear_path.back() == 0)
83 pose_array.poses.push_back(pose);
97 std::vector<double> path_array;
98 std::vector<double> path_matrix[2];
100 geometry_msgs::Pose
pose;
123 path_array = path_matrix[0];
130 for (
unsigned int i = 0; i < path_array.size(); i++)
138 q.
setRPY(0, -path_array.at(i), 0);
143 q.
setRPY(0, path_array.at(i), 0);
154 pose_array.poses.push_back(pose);
geometry_msgs::Pose start
Quaternion slerp(const Quaternion &q, const tfScalar &t) const
bool linearInterpolation(geometry_msgs::PoseArray &pose_array, const cob_cartesian_controller::CartesianActionStruct as)
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
geometry_msgs::Pose pose_center
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
static TrajectoryProfileBase * createProfile(const cob_cartesian_controller::CartesianActionStruct ¶ms)
bool circularInterpolation(geometry_msgs::PoseArray &pose_array, const cob_cartesian_controller::CartesianActionStruct as)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
tfScalar angleShortestPath(const Quaternion &q) const
boost::shared_ptr< TrajectoryProfileBase > trajectory_profile_generator_
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
static void pointTFToMsg(const Point &bt_v, geometry_msgs::Point &msg_v)