genericArmCtrl.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 <cob_trajectory_controller/genericArmCtrl.h>
19 #include <fstream>
20 #include <cob_trajectory_controller/RefValJS_PTP_Trajectory.h>
21 #include <cob_trajectory_controller/RefValJS_PTP.h>
22 
23 
24 /********************************************************************
25  * Some macros for error checking and handling *
26  ********************************************************************/
27 
28 
29 
30 #define STD_CHECK_PROCEDURE() \
31 if ( isMoving ) \
32 { \
33  ROS_WARN("Manipulator still in motion, call stop first!"); \
34  return false; \
35 } \
36 if (m_pRefVals != NULL) { \
37  delete m_pRefVals; \
38  m_pRefVals = NULL; \
39 }
40 
41 
42 #define START_CTRL_JOB(info) \
43 if ( m_cThread.startJob(m_pRefVals) == false ) { \
44  m_ErrorMsg.assign("Internal Error: Unable to start movement, other control Job still active."); \
45  return false; \
46 } else { \s \
47  return true; \
48 }
49 
50 
51 
52 
53 /********************************************************************
54  * Initialisierungsfunktionen *
55  ********************************************************************/
56 
57 
58 genericArmCtrl::genericArmCtrl(int DOF, double PTPvel, double PTPacc, double maxError)
59 {
60 
61  m_DOF = DOF;
62 
63  m_pRefVals = NULL;
64 
65  isMoving = false;
66 
67  //TODO: make configurable
68  SetPTPvel(PTPvel);
69  SetPTPacc(PTPacc);
70 
71  //m_P = 2.5;
72  m_P = 4.0;
73  m_Vorsteuer = 0.9;
74  m_AllowedError = maxError;//0.5;//0.25; // rad
75  m_CurrentError = 0.0; // rad
76  m_TargetError = 0.02; // rad;
77  overlap_time = 0.4;
78  m_ExtraTime = 3; // s
79 
80 }
81 
82 
83 genericArmCtrl::~genericArmCtrl()
84 {
85  if (m_pRefVals != NULL)
86  delete m_pRefVals;
87 }
88 
89 /********************************************************************
90  * Parameter Abfragen *
91  ********************************************************************/
92 
93 std::vector<double> genericArmCtrl::GetPTPvel() const
94  { return m_vel_js; }
95 
96 std::vector<double> genericArmCtrl::GetPTPacc() const
97  { return m_acc_js; }
98 
99 
100 
101 
102 /********************************************************************
103  * Parameter Setzen *
104  ********************************************************************/
105 
106 void genericArmCtrl::SetPTPvel(double vel)
107 {
108  m_vel_js.resize(m_DOF);
109  for (int i = 0; i < m_DOF; i++)
110  m_vel_js.at(i) = vel;
111 }
112 
113 void genericArmCtrl::SetPTPacc(double acc)
114 {
115  m_acc_js.resize(m_DOF);
116  for (int i = 0; i < m_DOF; i++)
117  m_acc_js.at(i) = acc;
118 }
119 
120 /********************************************************************
121  * PTP (Joint Space) Motion: *
122  ********************************************************************/
123 
125 bool genericArmCtrl::moveThetas(std::vector<double> conf_goal, std::vector<double> conf_current)
126 {
127  /* Prüfen ob Arm noch in Bewegung & Prüfen ob alte Sollwerte gelöscht werden müssen: */
128  STD_CHECK_PROCEDURE()
129 
130  /* Sollwerte generieren: */
131  double vel = 10000;
132  for (unsigned int i = 0; i<m_vel_js.size(); i++)
133  {
134  if(m_vel_js.at(i) < vel)
135  vel = m_vel_js.at(i);
136  }
137  double acc = 10000;
138  for (unsigned int i = 0; i<m_acc_js.size(); i++)
139  {
140  if(m_acc_js.at(i) < acc)
141  acc = m_acc_js.at(i);
142  }
143 
144  ;
145  m_pRefVals = new RefValJS_PTP(conf_current, conf_goal, vel, acc);
146  startTime_.SetNow();
147  isMoving = true;
148  TotalTime_ = m_pRefVals->getTotalTime();
149  ROS_INFO("Starting control of trajectory: %f s long", TotalTime_);
150 
151  return true; //TODO when to return false?
152 }
153 
154 bool genericArmCtrl::moveTrajectory(trajectory_msgs::JointTrajectory pfad, std::vector<double> conf_current)
155 {
156  /* Prüfen, ob erster Punkt nah genug an momentaner Position ist: */
157  if (pfad.points.size() == 2)
158  {
159  return moveThetas(pfad.points[1].positions, conf_current);
160  }
161  for(unsigned int i = 0; i < pfad.points.front().positions.size(); i++ )
162  {
163  if((pfad.points.front().positions.at(i) - conf_current.at(i)) > 0.25)
164  {
165  ROS_ERROR("Cannot start trajectory motion, manipulator not in trajectory start position.");
166  return false;
167  }
168  }
169 
170  /* Prüfen ob Arm noch in Bewegung & Prüfen ob alte Sollwerte gelöscht werden müssen: */
171  STD_CHECK_PROCEDURE()
172 
173  /* Sollwerte generieren: */
174  double vel = m_vel_js.at(0);
175  for (unsigned int i = 0; i<m_vel_js.size(); i++)
176  {
177  if(m_vel_js.at(i) < vel)
178  vel = m_vel_js.at(i);
179  }
180  double acc = m_acc_js.at(0);
181  for (unsigned int i = 0; i<m_acc_js.size(); i++)
182  {
183  if(m_acc_js.at(i) < acc)
184  acc = m_acc_js.at(i);
185  }
186  m_pRefVals = new RefValJS_PTP_Trajectory(pfad, vel, acc, true);
187 
188  /* Regeljob starten: */
189  startTime_.SetNow();
190  isMoving = true;
191  TotalTime_ = m_pRefVals->getTotalTime();
192  ROS_INFO("Starting control of trajectory: %f s long", TotalTime_);
193 
194  //START_CTRL_JOB(Trajectory)
195  return true;
196 }
197 
198 //bool genericArmCtrl::movePos(AbsPos position)
199 //{
200 // /* Prüfen ob Arm noch in Bewegung & Prüfen ob alte Sollwerte gelöscht werden müssen: */
201 // STD_CHECK_PROCEDURE()
202 //
203 // Jointd start = getCurrentAngles();
204 //
205 // // attempt to move along straight line:
206 // RefValJS_CartesianStraight* rv = new RefValJS_CartesianStraight(m_pKin, start);
207 // m_pRefVals = rv;
208 // if ( rv->calculate(position, m_vel_cartesian, m_acc_cartesian) == false )
209 // {
210 // m_ErrorMsg.assign("MovePos: Target Position not reachable.");
211 // #ifdef GENERIC_ARM_CTRL_LOG
212 // m_log << m_ErrorMsg << "\n";
213 // #endif
214 // return false;
215 // }
216 //
217 // /* Regeljob starten: */
218 // START_CTRL_JOB(Cartesian)
219 //}
220 
221 bool genericArmCtrl::step(std::vector<double> current_pos, std::vector<double> & desired_vel)
222 {
223  if(isMoving)
224  {
225  TimeStamp timeNow_;
226  timeNow_.SetNow();
227 
228  if ( m_pRefVals == NULL )
229  return false;
230  double t = timeNow_ - startTime_;
231  std::vector<double> m_qsoll = m_pRefVals->r_t( t );
232  std::vector<double> m_vsoll = m_pRefVals->dr_dt(t);
233 
234  if(t < TotalTime_)
235  {
236  if(t < overlap_time)
237  {
238  last_q = m_pRefVals->r_t( 0.0 );
239  last_q1 = m_pRefVals->r_t( 0.0 );
240  last_q2 = m_pRefVals->r_t( 0.0 );
241  last_q3 = m_pRefVals->r_t( 0.0 );
242  }
243  else
244  {
245  last_q = m_pRefVals->r_t( t - overlap_time );
246  last_q1 = m_pRefVals->r_t( t - (3.0*overlap_time/4.0) );
247  last_q2 = m_pRefVals->r_t( t - (overlap_time/2.0) );
248  last_q3 = m_pRefVals->r_t( t - (overlap_time/4.0) );
249  }
250  }
251 
252  double len = 0;
253  for(int i = 0; i < m_DOF; i++)
254  {
255  len += (m_qsoll.at(i) - current_pos.at(i)) * (m_qsoll.at(i) - current_pos.at(i));
256  }
257  m_CurrentError = sqrt(len);
258  if ( m_pRefVals != NULL && t < TotalTime_ + m_ExtraTime && (m_CurrentError > m_TargetError || t < TotalTime_) )
259  {
260  if ( m_CurrentError >= m_AllowedError )
261  {
262  ROS_ERROR("Current control error exceeds limit: %f >= %f", m_CurrentError, m_AllowedError);
263  //TODO: make generic to avoid out of bound exception
264  //ROS_ERROR("Current Soll: %f %f %f %f %f %f %f ", m_qsoll.at(0), m_qsoll.at(1), m_qsoll.at(2), m_qsoll.at(3), m_qsoll.at(4), m_qsoll.at(5), m_qsoll.at(6));
265  //ROS_ERROR("Current Ist: %f %f %f %f %f %f %f ", current_pos.at(0), current_pos.at(1), current_pos.at(2), current_pos.at(3), current_pos.at(4), current_pos.at(5), current_pos.at(6));
266  isMoving = false;
267  return false;
268  }
269  desired_vel.resize(m_DOF);
270  /* Vorsteuerung + P-Lageregler: */
271  for(int i = 0; i < m_DOF; i++)
272  {
273  desired_vel.at(i) = m_vsoll.at(i) * m_Vorsteuer + ( m_qsoll.at(i) - current_pos.at(i) ) * m_P;
274  }
275 
276  return true;
277  }
278  else /* entweder sind keine Sollwerte vorhanden, oder Zeit ist abgelaufen */
279  {
280  ROS_INFO("Probably finished trajectory");
281  isMoving = false;
282  desired_vel.resize(m_DOF);
283  for(int i = 0; i < m_DOF; i++)
284  {
285  desired_vel.at(i) = 0.0;
286  }
287  return true;
288  }
289  }
290  return false;
291 }
292 
293 
294 
295 
RefValJS_PTP_Trajectory
Definition: RefValJS_PTP_Trajectory.h:27
genericArmCtrl::SetPTPvel
void SetPTPvel(double vel)
Definition: genericArmCtrl.cpp:106
genericArmCtrl::m_TargetError
double m_TargetError
Definition: genericArmCtrl.h:75
genericArmCtrl::m_pRefVals
RefVal_JS * m_pRefVals
Definition: genericArmCtrl.h:59
genericArmCtrl::last_q
std::vector< double > last_q
Definition: genericArmCtrl.h:62
genericArmCtrl::step
bool step(std::vector< double > current_pos, std::vector< double > &desired_vel)
Definition: genericArmCtrl.cpp:221
genericArmCtrl::moveThetas
bool moveThetas(std::vector< double > conf_goal, std::vector< double > conf_current)
Will move the component to a goal configuration in Joint Space.
Definition: genericArmCtrl.cpp:125
RefValJS_PTP.h
genericArmCtrl::m_vel_js
std::vector< double > m_vel_js
Definition: genericArmCtrl.h:61
STD_CHECK_PROCEDURE
#define STD_CHECK_PROCEDURE()
Definition: genericArmCtrl.cpp:30
genericArmCtrl::m_acc_js
std::vector< double > m_acc_js
Definition: genericArmCtrl.h:66
TimeStamp::SetNow
void SetNow()
Makes time measurement.
Definition: TimeStamp.cpp:27
RefValJS_PTP
Definition: RefValJS_PTP.h:26
genericArmCtrl::moveTrajectory
bool moveTrajectory(trajectory_msgs::JointTrajectory pfad, std::vector< double > conf_current)
Definition: genericArmCtrl.cpp:154
genericArmCtrl::genericArmCtrl
genericArmCtrl(int DOF, double PTPvel=0.7, double PTPacc=0.2, double maxError=0.7)
Definition: genericArmCtrl.cpp:58
genericArmCtrl::m_CurrentError
double m_CurrentError
Definition: genericArmCtrl.h:74
ROS_INFO
#define ROS_INFO(...)
RefVal_JS::getTotalTime
virtual double getTotalTime() const =0
genericArmCtrl::GetPTPacc
std::vector< double > GetPTPacc() const
Definition: genericArmCtrl.cpp:96
genericArmCtrl::last_q2
std::vector< double > last_q2
Definition: genericArmCtrl.h:64
RefVal_JS::r_t
virtual std::vector< double > r_t(double t) const
Definition: RefVal_JS.h:29
genericArmCtrl::GetPTPvel
std::vector< double > GetPTPvel() const
Definition: genericArmCtrl.cpp:93
genericArmCtrl::SetPTPacc
void SetPTPacc(double acc)
Definition: genericArmCtrl.cpp:113
genericArmCtrl::last_q3
std::vector< double > last_q3
Definition: genericArmCtrl.h:65
RefValJS_PTP_Trajectory.h
RefVal_JS::dr_dt
virtual std::vector< double > dr_dt(double t) const
Definition: RefVal_JS.h:33
genericArmCtrl::startTime_
TimeStamp startTime_
Definition: genericArmCtrl.h:69
ROS_ERROR
#define ROS_ERROR(...)
genericArmCtrl::last_q1
std::vector< double > last_q1
Definition: genericArmCtrl.h:63
genericArmCtrl::m_AllowedError
double m_AllowedError
Definition: genericArmCtrl.h:73

cob_trajectory_controller
Author(s): Alexander Bubeck
autogenerated on Thu Apr 8 2021 02:39:55