TypeIIRMLVelocityMethods.cpp
Go to the documentation of this file.
1 // ---------------------- Doxygen info ----------------------
38 // ----------------------------------------------------------
39 // For a convenient reading of this file's source code,
40 // please use a tab width of four characters.
41 // ----------------------------------------------------------
42 
43 
44 #include <TypeIIRMLVelocity.h>
45 #include <TypeIIRMLMath.h>
46 #include <TypeIIRMLDecisions.h>
47 #include <RMLVelocityInputParameters.h>
48 #include <RMLVelocityOutputParameters.h>
49 #include <RMLVector.h>
50 #include <ReflexxesAPI.h>
51 #include <RMLVelocityFlags.h>
52 #include <math.h>
53 
54 
55 using namespace TypeIIRMLMath;
56 
57 
58 //****************************************************************************
59 // CalculateExecutionTimes()
60 
61 void TypeIIRMLVelocity::CalculateExecutionTimes(void)
62 {
63  unsigned int i = 0;
64 
65  for (i = 0; i < this->NumberOfDOFs; i++)
66  {
67  Polynomials[i].ValidPolynomials = 0;
68 
69  if ((this->CurrentInputParameters->SelectionVector->VecData)[i])
70  {
71  (this->ExecutionTimes->VecData)[i] = fabs( (this->CurrentInputParameters->CurrentVelocityVector->VecData )[i]
72  - (this->CurrentInputParameters->TargetVelocityVector->VecData )[i])
73  / this->CurrentInputParameters->MaxAccelerationVector->VecData[i];
74  }
75  }
76  return;
77 }
78 
79 
80 //****************************************************************************
81 // ComputeTrajectoryParameters()
82 
83 void TypeIIRMLVelocity::ComputeTrajectoryParameters(void)
84 {
85  unsigned int i = 0;
86 
87  double TimeForFirstSegment = 0.0;
88 
89  for (i = 0; i < this->NumberOfDOFs; i++)
90  {
91  if ((this->CurrentInputParameters->SelectionVector->VecData)[i])
92  {
93  (this->Polynomials)[i].ValidPolynomials = 0 ;
94 
95  TimeForFirstSegment = fabs( (this->CurrentInputParameters->CurrentVelocityVector->VecData )[i]
96  - (this->CurrentInputParameters->TargetVelocityVector->VecData )[i])
97  / this->CurrentInputParameters->MaxAccelerationVector->VecData[i];
98 
99  // if vi <= vtrgt
100  if (Decision_V___001( (this->CurrentInputParameters->CurrentVelocityVector->VecData )[i]
101  , (this->CurrentInputParameters->TargetVelocityVector->VecData )[i]))
102  {
103  (this->Polynomials)[i].PositionPolynomial[(this->Polynomials)[i].ValidPolynomials].SetCoefficients((0.5 * (this->CurrentInputParameters->MaxAccelerationVector->VecData)[i]), (this->CurrentInputParameters->CurrentVelocityVector->VecData)[i], (this->CurrentInputParameters->CurrentPositionVector->VecData)[i], 0.0);
104  (this->Polynomials)[i].VelocityPolynomial[(this->Polynomials)[i].ValidPolynomials].SetCoefficients(0.0, (this->CurrentInputParameters->MaxAccelerationVector->VecData)[i], (this->CurrentInputParameters->CurrentVelocityVector->VecData)[i], 0.0);
105  (this->Polynomials)[i].AccelerationPolynomial[(this->Polynomials)[i].ValidPolynomials].SetCoefficients(0.0, 0.0, (this->CurrentInputParameters->MaxAccelerationVector->VecData)[i], 0.0);
106 
107  (this->CurrentInputParameters->CurrentPositionVector->VecData)[i] += (this->CurrentInputParameters->CurrentVelocityVector->VecData )[i]
108  * TimeForFirstSegment
109  + 0.5
110  * (this->CurrentInputParameters->MaxAccelerationVector->VecData)[i]
111  * pow2(TimeForFirstSegment);
112 
113  (this->CurrentInputParameters->CurrentVelocityVector->VecData)[i] += (this->CurrentInputParameters->MaxAccelerationVector->VecData)[i]
114  * TimeForFirstSegment;
115  }
116  else
117  {
118  (this->Polynomials)[i].PositionPolynomial[(this->Polynomials)[i].ValidPolynomials].SetCoefficients((-0.5 * (this->CurrentInputParameters->MaxAccelerationVector->VecData)[i]), (this->CurrentInputParameters->CurrentVelocityVector->VecData)[i], (this->CurrentInputParameters->CurrentPositionVector->VecData)[i], 0.0);
119  (this->Polynomials)[i].VelocityPolynomial[(this->Polynomials)[i].ValidPolynomials].SetCoefficients(0.0, -(this->CurrentInputParameters->MaxAccelerationVector->VecData)[i], (this->CurrentInputParameters->CurrentVelocityVector->VecData)[i], 0.0);
120  (this->Polynomials)[i].AccelerationPolynomial[(this->Polynomials)[i].ValidPolynomials].SetCoefficients(0.0, 0.0, -(this->CurrentInputParameters->MaxAccelerationVector->VecData)[i], 0.0);
121 
122  (this->CurrentInputParameters->CurrentPositionVector->VecData)[i] += (this->CurrentInputParameters->CurrentVelocityVector->VecData )[i]
123  * TimeForFirstSegment
124  - 0.5
125  * (this->CurrentInputParameters->MaxAccelerationVector->VecData)[i]
126  * pow2(TimeForFirstSegment);
127 
128  (this->CurrentInputParameters->CurrentVelocityVector->VecData)[i] += -(this->CurrentInputParameters->MaxAccelerationVector->VecData)[i]
129  * TimeForFirstSegment;
130  }
131 
132  (this->Polynomials)[i].PolynomialTimes[(this->Polynomials)[i].ValidPolynomials] = TimeForFirstSegment;
133  (this->Polynomials)[i].ValidPolynomials++;
134 
135  (this->OutputParameters->ExecutionTimes->VecData)[i] = TimeForFirstSegment;
136 
137  (this->OutputParameters->PositionValuesAtTargetVelocity->VecData)[i] = (this->CurrentInputParameters->CurrentPositionVector->VecData)[i];
138 
139  if (TimeForFirstSegment > this->SynchronizationTime)
140  {
141  this->OutputParameters->DOFWithTheGreatestExecutionTime = i;
142  this->SynchronizationTime = TimeForFirstSegment;
143  }
144 
145  // final segment to hold the velocity
146 
147  (this->Polynomials)[i].PositionPolynomial[(this->Polynomials)[i].ValidPolynomials].SetCoefficients(0.0, (this->CurrentInputParameters->CurrentVelocityVector->VecData)[i], (this->CurrentInputParameters->CurrentPositionVector->VecData)[i], TimeForFirstSegment);
148  (this->Polynomials)[i].VelocityPolynomial[(this->Polynomials)[i].ValidPolynomials].SetCoefficients(0.0, 0.0, (this->CurrentInputParameters->CurrentVelocityVector->VecData)[i], TimeForFirstSegment);
149  (this->Polynomials)[i].AccelerationPolynomial[(this->Polynomials)[i].ValidPolynomials].SetCoefficients(0.0, 0.0, 0.0, TimeForFirstSegment);
150 
151  (this->Polynomials)[i].PolynomialTimes[(this->Polynomials)[i].ValidPolynomials] = TimeForFirstSegment + RML_INFINITY;
152  (this->Polynomials)[i].ValidPolynomials++;
153 
154  // now, all polynomials are set up, and the desired values can be calculated.
155 
156  }
157  else
158  {
159  (this->Polynomials)[i].ValidPolynomials = 0;
160  }
161  }
162 
163  return;
164 }
165 
166 
167 //****************************************************************************
168 // ComputePhaseSynchronizationParameters()
169 
170 void TypeIIRMLVelocity::ComputePhaseSynchronizationParameters(void)
171 {
172  unsigned int i = 0;
173 
174  double VectorStretchFactorMaxAcceleration = 0.0
175  , PhaseSyncTimeAverage = 0.0
176  , PhaseSyncDOFCounter = 0.0;
177 
178  this->SetupPhaseSyncSelectionVector();
179 
180  if (this->CurrentTrajectoryIsPhaseSynchronized)
181  {
182  this->CurrentTrajectoryIsPhaseSynchronized = this->IsPhaseSynchronizationPossible();
183 
184  if ( (this->CurrentTrajectoryIsPhaseSynchronized)
185  && (fabs((this->PhaseSynchronizationReferenceVector->VecData)[
186  this->OutputParameters->DOFWithTheGreatestExecutionTime])
187  > ABSOLUTE_PHASE_SYNC_EPSILON) )
188  {
189  VectorStretchFactorMaxAcceleration = (this->CurrentInputParameters->MaxAccelerationVector->VecData)[this->OutputParameters->DOFWithTheGreatestExecutionTime]
190  / fabs((this->PhaseSynchronizationReferenceVector->VecData)[this->OutputParameters->DOFWithTheGreatestExecutionTime]);
191 
192  for(i = 0; i < this->NumberOfDOFs; i++)
193  {
194  if((this->PhaseSyncSelectionVector->VecData)[i])
195  {
196  (this->ExecutionTimes->VecData)[i] = 0.0;
197 
198  (this->PhaseSynchronizationMaxAccelerationVector->VecData)[i] = fabs(VectorStretchFactorMaxAcceleration
199  * (this->PhaseSynchronizationReferenceVector->VecData)[i]);
200  if ( (this->PhaseSynchronizationMaxAccelerationVector->VecData)[i] <= 0.0 )
201  {
202  (this->PhaseSynchronizationMaxAccelerationVector->VecData)[i] = POSITIVE_ZERO;
203  }
204 
205  if ( (this->PhaseSynchronizationMaxAccelerationVector->VecData)[i]
206  > ( (this->CurrentInputParameters->MaxAccelerationVector->VecData)[i] * ( 1.0 + RELATIVE_PHASE_SYNC_EPSILON ) + ABSOLUTE_PHASE_SYNC_EPSILON ) )
207  {
208  this->CurrentTrajectoryIsPhaseSynchronized = false;
209  break;
210  }
211  }
212  }
213  }
214  else
215  {
216  this->CurrentTrajectoryIsPhaseSynchronized = false;
217  }
218 
219  if (this->CurrentTrajectoryIsPhaseSynchronized)
220  {
221  for(i = 0; i < this->NumberOfDOFs; i++)
222  {
223  if((this->PhaseSyncSelectionVector->VecData)[i])
224  {
225  (this->ExecutionTimes->VecData)[i] = fabs( (this->CurrentInputParameters->CurrentVelocityVector->VecData)[i]
226  - (this->CurrentInputParameters->TargetVelocityVector->VecData)[i])
227  / (this->PhaseSynchronizationMaxAccelerationVector->VecData)[i];
228  }
229  }
230 
231  PhaseSyncTimeAverage = 0.0;
232  PhaseSyncDOFCounter = 0.0;
233 
234  for(i = 0; i < this->NumberOfDOFs; i++)
235  {
236  if((this->PhaseSyncSelectionVector->VecData)[i])
237  {
238  PhaseSyncTimeAverage += (this->ExecutionTimes->VecData)[i];
239  PhaseSyncDOFCounter += 1.0;
240  }
241  }
242 
243  if (PhaseSyncDOFCounter > 0.0)
244  {
245  PhaseSyncTimeAverage /= PhaseSyncDOFCounter;
246 
247  for(i = 0; i < this->NumberOfDOFs; i++)
248  {
249  if ((this->PhaseSyncSelectionVector->VecData)[i])
250  {
251  if ( fabs((this->ExecutionTimes->VecData)[i] - PhaseSyncTimeAverage)
252  > (ABSOLUTE_PHASE_SYNC_EPSILON + RELATIVE_PHASE_SYNC_EPSILON * PhaseSyncTimeAverage) )
253  {
254  this->CurrentTrajectoryIsPhaseSynchronized = false;
255  break;
256  }
257  }
258  }
259  }
260  }
261  }
262 
263  if (this->CurrentTrajectoryIsPhaseSynchronized)
264  {
265  for (i = 0; i < this->NumberOfDOFs; i++)
266  {
267  if ((this->PhaseSyncSelectionVector->VecData)[i])
268  {
269  (this->CurrentInputParameters->MaxAccelerationVector->VecData)[i]
270  = (this->PhaseSynchronizationMaxAccelerationVector->VecData)[i];
271  }
272  }
273  }
274 
275  return;
276 }
277 
278 
279 //****************************************************************************
280 // ComputeAndSetOutputParameters()
281 
282 int TypeIIRMLVelocity::ComputeAndSetOutputParameters( const double &TimeValueInSeconds
283  , RMLVelocityOutputParameters *OP ) const
284 {
285  unsigned int i = 0;
286 
287  int j = 0
288  , ReturnValueForThisMethod = ReflexxesAPI::RML_FINAL_STATE_REACHED;
289 
290  // calculate the new state of motion
291 
292  for (i = 0; i < this->NumberOfDOFs; i++)
293  {
294  if ((this->CurrentInputParameters->SelectionVector->VecData)[i])
295  {
296  j = 0;
297 
298  while ( (TimeValueInSeconds > (((this->Polynomials)[i].PolynomialTimes)[j])) && (j < MAXIMAL_NO_OF_POLYNOMIALS))
299  {
300  j++;
301  }
302 
303  (OP->NewPositionVector->VecData)[i]
304  = (this->Polynomials)[i].PositionPolynomial[j].CalculateValue(TimeValueInSeconds);
305  (OP->NewVelocityVector->VecData)[i]
306  = (this->Polynomials)[i].VelocityPolynomial[j].CalculateValue(TimeValueInSeconds);
307  (OP->NewAccelerationVector->VecData)[i]
308  = (this->Polynomials)[i].AccelerationPolynomial[j].CalculateValue(TimeValueInSeconds);
309 
310  if ( j < ((this->Polynomials)[i].ValidPolynomials) - 1)
311  {
312  ReturnValueForThisMethod = ReflexxesAPI::RML_WORKING;
313  }
314 
315  (OP->PositionValuesAtTargetVelocity->VecData)[i]
316  = (this->Polynomials)[i].PositionPolynomial[(this->Polynomials)[i].ValidPolynomials - 1].a0;
317  }
318  else
319  {
320  (OP->NewPositionVector->VecData)[i]
321  = (this->CurrentInputParameters->CurrentPositionVector->VecData)[i];
322  (OP->NewVelocityVector->VecData)[i]
323  = (this->CurrentInputParameters->CurrentVelocityVector->VecData)[i];
324  (OP->NewAccelerationVector->VecData)[i]
325  = (this->CurrentInputParameters->CurrentAccelerationVector->VecData)[i];
326  (OP->PositionValuesAtTargetVelocity->VecData)[i]
327  = (this->CurrentInputParameters->CurrentPositionVector->VecData)[i];
328  }
329  }
330 
331  return(ReturnValueForThisMethod);
332 }
RMLVector.h
Header file for the dynamic vector class used for the Reflexxes Motion Libraries. ...
ABSOLUTE_PHASE_SYNC_EPSILON
#define ABSOLUTE_PHASE_SYNC_EPSILON
Absolute epsilon to check whether all required vectors are collinear.
Definition: TypeIIRMLMath.h:179
ReflexxesAPI.h
Header file for the class ReflexxesAPI (API of the Reflexxes Motion Libraries)
RMLOutputParameters::NewAccelerationVector
RMLDoubleVector * NewAccelerationVector
A pointer to the new acceleration vector .
Definition: RMLOutputParameters.h:1555
MAXIMAL_NO_OF_POLYNOMIALS
#define MAXIMAL_NO_OF_POLYNOMIALS
The maximum number of polynomials.
Definition: TypeIIRMLMath.h:115
TypeIIRMLVelocity.h
Header file for the class TypeIIRMLVelocity.
RMLVector::VecData
T * VecData
Pointer to the actual vector data, that is, an array of type T objects.
Definition: RMLVector.h:524
TypeIIRMLVelocity::ComputeAndSetOutputParameters
int ComputeAndSetOutputParameters(const double &TimeValueInSeconds, RMLVelocityOutputParameters *OP) const
Computes the output values of the velocity-based On-line Trajectory Generation algorithm, that is, the state of motion for the next control cycle.
Definition: TypeIIRMLVelocityMethods.cpp:282
TypeIIRMLDecisions.h
Header file for decisions of the two decision trees of the Type II On-Line Trajectory Generation algo...
RMLOutputParameters::NewPositionVector
RMLDoubleVector * NewPositionVector
A pointer to the new position vector .
Definition: RMLOutputParameters.h:1523
TypeIIRMLMath::Decision_V___001
bool Decision_V___001(const double &CurrentVelocity, const double &TargetVelocity)
Is (vi <= vtrgt)?
Definition: TypeIIRMLDecisions.cpp:485
TypeIIRMLVelocity::ComputeTrajectoryParameters
void ComputeTrajectoryParameters(void)
Computes all trajectory parameters for non- and phase- synchronized trajectories. ...
Definition: TypeIIRMLVelocityMethods.cpp:83
TypeIIRMLMath.h
Header file for functions and definitions of constant values and macros.
RML_INFINITY
#define RML_INFINITY
A value for infinity .
Definition: TypeIIRMLMath.h:201
RMLVelocityInputParameters.h
Header file for the class RMLVelocityInputParameters.
RMLVelocityFlags.h
Header file for the class RMLVelocityFlags.
RMLVelocityOutputParameters.h
Header file for the class RMLVelocityOutputParameters.
RMLVelocityOutputParameters
Class for the output parameters of the velocity-based On-Line Trajectory Generation algorithm...
Definition: RMLVelocityOutputParameters.h:73
TypeIIRMLVelocity::ComputePhaseSynchronizationParameters
void ComputePhaseSynchronizationParameters(void)
Checks, whether phase-synchronization is possible. If possible the corresponding vectors are computed...
Definition: TypeIIRMLVelocityMethods.cpp:170
pow2
#define pow2(A)
A to the power of 2.
Definition: TypeIIRMLMath.h:273
TypeIIRMLVelocity::CalculateExecutionTimes
void CalculateExecutionTimes(void)
Calculates the minimum execution times and corresponding motion profiles of each selected degree of f...
Definition: TypeIIRMLVelocityMethods.cpp:61
RMLOutputParameters::NewVelocityVector
RMLDoubleVector * NewVelocityVector
A pointer to the new velocity vector .
Definition: RMLOutputParameters.h:1539
RELATIVE_PHASE_SYNC_EPSILON
#define RELATIVE_PHASE_SYNC_EPSILON
Relative epsilon to check whether all required vectors are collinear.
Definition: TypeIIRMLMath.h:192
RMLVelocityOutputParameters::PositionValuesAtTargetVelocity
RMLDoubleVector * PositionValuesAtTargetVelocity
A pointer to an RMLDoubleVector object that contains the position values at the instants the desired ...
Definition: RMLVelocityOutputParameters.h:322
POSITIVE_ZERO
#define POSITIVE_ZERO
To prevent from numerical errors, a value for a "positive" value of zero is required for deterministi...
Definition: TypeIIRMLMath.h:164

libreflexxestype2
Author(s):
autogenerated on Sat Nov 21 2020 03:17:34