trajectory_interpolator.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 
18 #include <math.h>
19 #include <vector>
20 
23 
24 bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_array,
26 {
28 
29  pose_array.header.stamp = ros::Time::now();
30  pose_array.header.frame_id = root_frame_;
31 
32  tf::Quaternion q_start, q_end;
33 
34  std::vector<double> linear_path, angular_path, path;
35  std::vector<double> path_matrix[2];
36  geometry_msgs::Pose pose;
37 
38  double norm_factor;
39  tf::quaternionMsgToTF(as.move_lin.start.orientation, q_start);
40  tf::quaternionMsgToTF(as.move_lin.end.orientation, q_end);
41 
42  double Se_lin = sqrt(pow((as.move_lin.end.position.x - as.move_lin.start.position.x), 2) +
43  pow((as.move_lin.end.position.y - as.move_lin.start.position.y), 2) +
44  pow((as.move_lin.end.position.z - as.move_lin.start.position.z), 2));
45 
46  double Se_rot = q_start.angleShortestPath(q_end);
47 
48  if (!trajectory_profile_generator_->calculateProfile(path_matrix, Se_lin, Se_rot))
49  {
50  return false;
51  }
52 
53  linear_path = path_matrix[0];
54  angular_path = path_matrix[1];
55 
56  if (fabs(linear_path.back()) > fabs(angular_path.back()))
57  {
58  path = linear_path;
59  }
60  else
61  {
62  path = angular_path;
63  }
64  norm_factor = 1/path.back();
65 
66  // Interpolate the linear path
67  for (unsigned int i = 0; i < linear_path.size(); i++)
68  {
69  if (linear_path.back() == 0)
70  {
71  pose.position.x = as.move_lin.start.position.x;
72  pose.position.y = as.move_lin.start.position.y;
73  pose.position.z = as.move_lin.start.position.z;
74  }
75  else
76  {
77  pose.position.x = as.move_lin.start.position.x + linear_path.at(i) * (as.move_lin.end.position.x - as.move_lin.start.position.x) / linear_path.back();
78  pose.position.y = as.move_lin.start.position.y + linear_path.at(i) * (as.move_lin.end.position.y - as.move_lin.start.position.y) / linear_path.back();
79  pose.position.z = as.move_lin.start.position.z + linear_path.at(i) * (as.move_lin.end.position.z - as.move_lin.start.position.z) / linear_path.back();
80  }
81 
82  tf::quaternionTFToMsg(q_start.slerp(q_end, path.at(i) * norm_factor), pose.orientation);
83  pose_array.poses.push_back(pose);
84  }
85  return true;
86 }
87 
88 bool TrajectoryInterpolator::circularInterpolation(geometry_msgs::PoseArray& pose_array,
90 {
91  pose_array.header.stamp = ros::Time::now();
92  pose_array.header.frame_id = root_frame_;
93 
95  tf::Transform C, P, T;
96 
97  std::vector<double> path_array;
98  std::vector<double> path_matrix[2];
99 
100  geometry_msgs::Pose pose;
101 
102  double Se = as.move_circ.end_angle - as.move_circ.start_angle;
103 
104  bool forward;
105 
106  // Needed for the circle direction!
107  if (Se < 0)
108  {
109  forward = false;
110  }
111  else
112  {
113  forward = true;
114  }
115  Se = std::fabs(Se);
116 
117  // Calculates the Path with RAMP or SINOID profile
118  if (!this->trajectory_profile_generator_->calculateProfile(path_matrix, Se, 0))
119  {
120  return false;
121  }
122 
123  path_array = path_matrix[0];
124  // Define Center Pose
125  C.setOrigin(tf::Vector3(as.move_circ.pose_center.position.x, as.move_circ.pose_center.position.y, as.move_circ.pose_center.position.z));
126  tf::quaternionMsgToTF(as.move_circ.pose_center.orientation, q);
127  C.setRotation(q);
128 
129  // Interpolate the circular path
130  for (unsigned int i = 0; i < path_array.size(); i++)
131  {
132  // Rotate T
133  T.setOrigin(tf::Vector3(cos(path_array.at(i)) * as.move_circ.radius, 0, sin(path_array.at(i)) * as.move_circ.radius));
134 
135  if (forward)
136  {
137  T.setOrigin(tf::Vector3(cos(path_array.at(i)) * as.move_circ.radius, 0, sin(path_array.at(i)) * as.move_circ.radius));
138  q.setRPY(0, -path_array.at(i), 0);
139  }
140  else
141  {
142  T.setOrigin(tf::Vector3(cos(as.move_circ.start_angle - path_array.at(i)) * as.move_circ.radius, 0, sin(as.move_circ.start_angle - path_array.at(i)) * as.move_circ.radius));
143  q.setRPY(0, path_array.at(i), 0);
144  }
145 
146  T.setRotation(q);
147 
148  // Calculate TCP Position
149  P = C * T;
150 
151  tf::pointTFToMsg(P.getOrigin(), pose.position);
152  tf::quaternionTFToMsg(P.getRotation(), pose.orientation);
153 
154  pose_array.poses.push_back(pose);
155  }
156 
157  return true;
158 }
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
path
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)
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
static TrajectoryProfileBase * createProfile(const cob_cartesian_controller::CartesianActionStruct &params)
bool circularInterpolation(geometry_msgs::PoseArray &pose_array, const cob_cartesian_controller::CartesianActionStruct as)
Quaternion getRotation() const
static Time now()
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_
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
static void pointTFToMsg(const Point &bt_v, geometry_msgs::Point &msg_v)


cob_cartesian_controller
Author(s): Christoph Mark
autogenerated on Thu Apr 8 2021 02:40:13