trajectory_profile_generator_base.h
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 #ifndef COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_BASE_H
19 #define COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_BASE_H
20 
21 #include <vector>
22 #include <ros/ros.h>
25 
26 
27 #define MIN_VELOCITY_THRESHOLD 0.001
28 
30 {
31 public:
33  : params_(params)
34  {}
35 
37  {}
38 
39  virtual bool calculateProfile(std::vector<double>* path_matrix,
40  const double Se_lin, const double Se_rot)
41  {
43  std::vector<double> linear_path, angular_path;
44 
45  cob_cartesian_controller::PathArray lin(Se_lin, linear_path);
46  cob_cartesian_controller::PathArray rot(Se_rot, angular_path);
47 
49 
50  // Get the profile timings from the longest path
51  bool success = getProfileTimings(pm.getMaxSe(), 0, true, pt_max_);
52 
53  // Calculate the paths
54  for (unsigned int i = 0; i < pm.pm_.size(); i++)
55  {
56  generatePath(pm.pm_[i]);
57  }
58 
59  // Adjust the array length
60  // If no trajectory was interpolated, then this path array contains only one constant value.
61  // This constant value needs to be duplicated N_max times for matrix conversion purposes.
62  ccu.adjustArrayLength(pm.pm_);
63 
64  ccu.copyMatrix(path_matrix, pm.pm_);
65 
66  return true;
67  }
68 
69 protected:
71  {
72  std::vector<double> array;
74 
75  // Calculate the Profile Timings
76  if (getProfileTimings(pa.Se_, pt_max_.te, false, pt))
77  {
78  array = getTrajectory(pa.Se_, pt);
79  }
80  else
81  {
82  array.push_back(0);
83  }
84 
85  pa.array_ = array;
86  return true;
87  }
88 
89  virtual bool getProfileTimings(double Se, double te, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt) = 0;
90  virtual std::vector<double> getTrajectory(double se, cob_cartesian_controller::ProfileTimings pt) = 0;
91 
94 };
95 
96 #endif // COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_BASE_H
TrajectoryProfileBase(const cob_cartesian_controller::CartesianActionStruct &params)
const cob_cartesian_controller::CartesianActionStruct & params_
virtual bool calculateProfile(std::vector< double > *path_matrix, const double Se_lin, const double Se_rot)
virtual bool getProfileTimings(double Se, double te, bool calcMaxTe, cob_cartesian_controller::ProfileTimings &pt)=0
virtual std::vector< double > getTrajectory(double se, cob_cartesian_controller::ProfileTimings pt)=0
cob_cartesian_controller::ProfileTimings pt_max_
void adjustArrayLength(std::vector< cob_cartesian_controller::PathArray > &m)
void copyMatrix(std::vector< double > *path_array, std::vector< cob_cartesian_controller::PathArray > &m)
virtual bool generatePath(cob_cartesian_controller::PathArray &pa)


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