TypeIIRMLCalculatePositionalExtrems.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 <TypeIIRMLPosition.h>
45 #include <TypeIIRMLMath.h>
46 #include <TypeIIRMLPolynomial.h>
47 #include <RMLPositionOutputParameters.h>
48 #include <RMLPositionInputParameters.h>
49 
50 
51 
52 using namespace TypeIIRMLMath;
53 
54 
55 //*******************************************************************************************
56 // CalculatePositionalExtrems()
57 
58 void TypeIIRMLPosition::CalculatePositionalExtrems( const double &TimeValueInSeconds
59  , RMLPositionOutputParameters *OP ) const
60 {
61  unsigned int i = 0
62  , NumberOfRoots = 0
63  , k = 0
64  , l = 0;
65 
66  int j = 0;
67 
68  double AnalizedPosition = 0.0
69  , TimeOfZeroVelocity1 = 0.0
70  , TimeOfZeroVelocity2 = 0.0
71  , TimeValueAtExtremumPosition = 0.0;
72 
73  for (i = 0; i < this->NumberOfDOFs; i++)
74  {
75  if ((this->ModifiedSelectionVector->VecData)[i])
76  {
77  // Initially, set all extrema values to the current position values.
78  (OP->MinPosExtremaPositionVectorOnly->VecData)[i]
79  = (OP->NewPositionVector->VecData)[i];
80  (OP->MaxPosExtremaPositionVectorOnly->VecData)[i]
81  = (OP->NewPositionVector->VecData)[i];
82 
83  // Check all root of the velocity polynomials.
84  for (j = 0; j < ((this->Polynomials)[i].ValidPolynomials - 1); j++)
85  {
86  if ((this->Polynomials)[i].PolynomialTimes[j] > TimeValueInSeconds)
87  {
88  if ( Sign( (j == 0)
89  ? ((this->Polynomials)[i].VelocityPolynomial[j].CalculateValue(0.0))
90  : ((this->Polynomials)[i].VelocityPolynomial[j].CalculateValue(
91  (this->Polynomials)[i].PolynomialTimes[j - 1])))
92  !=
93  Sign( (this->Polynomials)[i].VelocityPolynomial[j].CalculateValue(
94  (this->Polynomials)[i].PolynomialTimes[j])) )
95  {
96  (this->Polynomials)[i].VelocityPolynomial[j].CalculateRealRoots( &NumberOfRoots
97  , &TimeOfZeroVelocity1
98  , &TimeOfZeroVelocity2);
99  if ((NumberOfRoots == 1) && (TimeOfZeroVelocity1 > TimeValueInSeconds))
100  {
101  AnalizedPosition = (this->Polynomials)[i].PositionPolynomial[j].CalculateValue(TimeOfZeroVelocity1);
102  TimeValueAtExtremumPosition = TimeOfZeroVelocity1;
103  }
104  else
105  {
106  continue;
107  }
108 
109  if ( (AnalizedPosition > (OP->MaxPosExtremaPositionVectorOnly->VecData)[i])
110  && (TimeValueAtExtremumPosition > TimeValueInSeconds) )
111  {
112  (OP->MaxPosExtremaPositionVectorOnly->VecData)[i] = AnalizedPosition;
113  (OP->MaxExtremaTimesVector->VecData)[i] = TimeValueAtExtremumPosition;
114  }
115 
116  if ( (AnalizedPosition < (OP->MinPosExtremaPositionVectorOnly->VecData)[i])
117  && (TimeValueAtExtremumPosition > TimeValueInSeconds) )
118  {
119  (OP->MinPosExtremaPositionVectorOnly->VecData)[i] = AnalizedPosition;
120  (OP->MinExtremaTimesVector->VecData)[i] = TimeValueAtExtremumPosition;
121  }
122  }
123  }
124  }
125 
126  // Check the target state of motion.
127  if ((this->Polynomials)[i].PolynomialTimes[(this->Polynomials)[i].ValidPolynomials - 2]
128  > TimeValueInSeconds)
129  {
130  if ( ((this->CurrentInputParameters->TargetPositionVector->VecData)[i]
131  >
132  (OP->MaxPosExtremaPositionVectorOnly->VecData)[i])
133  &&
134  (TimeValueInSeconds
135  <=
136  (this->Polynomials)[i].PolynomialTimes[(this->Polynomials)[i].ValidPolynomials - 2]))
137  {
138  (OP->MaxPosExtremaPositionVectorOnly->VecData)[i] = (this->CurrentInputParameters->TargetPositionVector->VecData)[i];
139 
140  if (this->CurrentTrajectoryIsNotSynchronized)
141  {
142  (OP->MaxExtremaTimesVector->VecData)[i] = this->MinimumExecutionTimes->VecData[i];
143  }
144  else
145  {
146  (OP->MaxExtremaTimesVector->VecData)[i] = this->SynchronizationTime + this->InternalClockInSeconds - this->CycleTime;
147  }
148  }
149 
150  if ( ((this->CurrentInputParameters->TargetPositionVector->VecData)[i]
151  <
152  (OP->MinPosExtremaPositionVectorOnly->VecData)[i])
153  &&
154  (TimeValueInSeconds
155  <=
156  (this->Polynomials)[i].PolynomialTimes[(this->Polynomials)[i].ValidPolynomials - 2]))
157  {
158  (OP->MinPosExtremaPositionVectorOnly->VecData)[i] = (this->CurrentInputParameters->TargetPositionVector->VecData)[i];
159 
160  if (this->CurrentTrajectoryIsNotSynchronized)
161  {
162  (OP->MinExtremaTimesVector->VecData)[i] = this->MinimumExecutionTimes->VecData[i];
163  }
164  else
165  {
166  (OP->MinExtremaTimesVector->VecData)[i] = this->SynchronizationTime + this->InternalClockInSeconds - this->CycleTime;
167  }
168  }
169  }
170 
171 
172  for (k = 0; k < this->NumberOfDOFs; k++)
173  {
174  if ((this->ModifiedSelectionVector->VecData)[k])
175  {
176  for (l = 0; l < MAXIMAL_NO_OF_POLYNOMIALS; l++)
177  {
178  if ( (this->Polynomials)[k].PolynomialTimes[l]
179  >=
180  (OP->MinExtremaTimesVector->VecData)[i])
181  {
182  break;
183  }
184  }
185 
186  (((OP->MinPosExtremaPositionVectorArray)[i])->VecData)[k]
187  = (this->Polynomials)[k].PositionPolynomial[l].CalculateValue(
188  (OP->MinExtremaTimesVector->VecData)[i]);
189  (((OP->MinPosExtremaVelocityVectorArray)[i])->VecData)[k]
190  = (this->Polynomials)[k].VelocityPolynomial[l].CalculateValue(
191  (OP->MinExtremaTimesVector->VecData)[i]);
192  (((OP->MinPosExtremaAccelerationVectorArray)[i])->VecData)[k]
193  = (this->Polynomials)[k].AccelerationPolynomial[l].CalculateValue(
194  (OP->MinExtremaTimesVector->VecData)[i]);
195 
196  (((OP->MinPosExtremaPositionVectorArray)[i])->VecData)[k]
197  = (this->CurrentInputParameters->TargetPositionVector->VecData)[k]
198  - ((this->StoredTargetPosition->VecData)[k]
199  - (((OP->MinPosExtremaPositionVectorArray)[i])->VecData)[k] );
200 
201  for (l = 0; l < MAXIMAL_NO_OF_POLYNOMIALS; l++)
202  {
203  if ( (this->Polynomials)[k].PolynomialTimes[l]
204  >=
205  (OP->MaxExtremaTimesVector->VecData)[i])
206  {
207  break;
208  }
209  }
210 
211  (((OP->MaxPosExtremaPositionVectorArray)[i])->VecData)[k]
212  = (this->Polynomials)[k].PositionPolynomial[l].CalculateValue(
213  (OP->MaxExtremaTimesVector->VecData)[i]);
214  (((OP->MaxPosExtremaVelocityVectorArray)[i])->VecData)[k]
215  = (this->Polynomials)[k].VelocityPolynomial[l].CalculateValue(
216  (OP->MaxExtremaTimesVector->VecData)[i]);
217  (((OP->MaxPosExtremaAccelerationVectorArray)[i])->VecData)[k]
218  = (this->Polynomials)[k].AccelerationPolynomial[l].CalculateValue(
219  (OP->MaxExtremaTimesVector->VecData)[i]);
220 
221  // Correct the position values (in order to cope with varying input values for the current
222  // position and the target position while the difference between them remains constant)
223  (((OP->MaxPosExtremaPositionVectorArray)[i])->VecData)[k]
224  = (this->CurrentInputParameters->TargetPositionVector->VecData)[k]
225  - ((this->StoredTargetPosition->VecData)[k]
226  - (((OP->MaxPosExtremaPositionVectorArray)[i])->VecData)[k] );
227  }
228  else
229  {
230  (((OP->MinPosExtremaPositionVectorArray)[i])->VecData)[k]
231  = (this->CurrentInputParameters->CurrentPositionVector->VecData)[k];
232  (((OP->MinPosExtremaVelocityVectorArray)[i])->VecData)[k]
233  = (this->CurrentInputParameters->CurrentVelocityVector->VecData)[k];
234  (((OP->MinPosExtremaAccelerationVectorArray)[i])->VecData)[k]
235  = (this->CurrentInputParameters->CurrentAccelerationVector->VecData)[k];
236 
237  (((OP->MaxPosExtremaPositionVectorArray)[i])->VecData)[k]
238  = (this->CurrentInputParameters->CurrentPositionVector->VecData)[k];
239  (((OP->MaxPosExtremaVelocityVectorArray)[i])->VecData)[k]
240  = (this->CurrentInputParameters->CurrentVelocityVector->VecData)[k];
241  (((OP->MaxPosExtremaAccelerationVectorArray)[i])->VecData)[k]
242  = (this->CurrentInputParameters->CurrentAccelerationVector->VecData)[k];
243  }
244  }
245 
246  OP->MaxExtremaTimesVector->VecData[i] -= TimeValueInSeconds;
247  if (OP->MaxExtremaTimesVector->VecData[i] < 0.0)
248  {
249  OP->MaxExtremaTimesVector->VecData[i] = 0.0;
250  }
251 
252  OP->MinExtremaTimesVector->VecData[i] -= TimeValueInSeconds;
253  if (OP->MinExtremaTimesVector->VecData[i] < 0.0)
254  {
255  OP->MinExtremaTimesVector->VecData[i] = 0.0;
256  }
257  }
258  else
259  {
260  // Set all non-selected DOFs to its current values.
261 
262  (OP->MinPosExtremaPositionVectorOnly->VecData)[i] = (this->CurrentInputParameters->CurrentPositionVector->VecData)[i];
263  (OP->MaxPosExtremaPositionVectorOnly->VecData)[i] = (this->CurrentInputParameters->CurrentPositionVector->VecData)[i];
264  (OP->MinExtremaTimesVector->VecData)[i] = 0.0;
265  (OP->MaxExtremaTimesVector->VecData)[i] = 0.0;
266 
267  for (k = 0; k < this->NumberOfDOFs; k++)
268  {
269  (((OP->MinPosExtremaPositionVectorArray)[i])->VecData)[k]
270  = (this->CurrentInputParameters->CurrentPositionVector->VecData)[k];
271  (((OP->MinPosExtremaVelocityVectorArray)[i])->VecData)[k]
272  = (this->CurrentInputParameters->CurrentVelocityVector->VecData)[k];
273  (((OP->MinPosExtremaAccelerationVectorArray)[i])->VecData)[k]
274  = (this->CurrentInputParameters->CurrentAccelerationVector->VecData)[k];
275 
276  (((OP->MaxPosExtremaPositionVectorArray)[i])->VecData)[k]
277  = (this->CurrentInputParameters->CurrentPositionVector->VecData)[k];
278  (((OP->MaxPosExtremaVelocityVectorArray)[i])->VecData)[k]
279  = (this->CurrentInputParameters->CurrentVelocityVector->VecData)[k];
280  (((OP->MaxPosExtremaAccelerationVectorArray)[i])->VecData)[k]
281  = (this->CurrentInputParameters->CurrentAccelerationVector->VecData)[k];
282  }
283  }
284  }
285 
286  return;
287 }
288 
289 
290 //*******************************************************************************************
291 // SetPositionalExtremsToZero()
292 
293 void TypeIIRMLPosition::SetPositionalExtremsToZero(RMLPositionOutputParameters *OP) const
294 {
295  unsigned int i = 0
296  , k = 0;
297 
298  for (i = 0; i < this->NumberOfDOFs; i++)
299  {
300  for (k = 0; k < this->NumberOfDOFs; k++)
301  {
302  (((OP->MinPosExtremaPositionVectorArray)[i])->VecData)[k]
303  = 0.0;
304  (((OP->MinPosExtremaVelocityVectorArray)[i])->VecData)[k]
305  = 0.0;
306  (((OP->MinPosExtremaAccelerationVectorArray)[i])->VecData)[k]
307  = 0.0;
308 
309  (((OP->MaxPosExtremaPositionVectorArray)[i])->VecData)[k]
310  = 0.0;
311  (((OP->MaxPosExtremaVelocityVectorArray)[i])->VecData)[k]
312  = 0.0;
313  (((OP->MaxPosExtremaAccelerationVectorArray)[i])->VecData)[k]
314  = 0.0;
315  }
316 
317  (OP->MinPosExtremaPositionVectorOnly->VecData)[i]
318  = 0.0;
319  (OP->MaxPosExtremaPositionVectorOnly->VecData)[i]
320  = 0.0;
321  (OP->MinExtremaTimesVector->VecData)[i]
322  = 0.0;
323  (OP->MaxExtremaTimesVector->VecData)[i]
324  = 0.0;
325  }
326 }
RMLPositionOutputParameters.h
Header file for the class RMLPositionOutputParameters.
TypeIIRMLPosition::CalculatePositionalExtrems
void CalculatePositionalExtrems(const double &TimeValueInSeconds, RMLPositionOutputParameters *OP) const
Set all positional extremum parameters of the output values of the algorithm (TypeIIRMLPosition::Outp...
Definition: TypeIIRMLCalculatePositionalExtrems.cpp:58
MAXIMAL_NO_OF_POLYNOMIALS
#define MAXIMAL_NO_OF_POLYNOMIALS
The maximum number of polynomials.
Definition: TypeIIRMLMath.h:115
RMLOutputParameters::MaxPosExtremaAccelerationVectorArray
RMLDoubleVector ** MaxPosExtremaAccelerationVectorArray
A pointer to an array of pointers to RMLDoubleVector objects. The number of array elements equals the...
Definition: RMLOutputParameters.h:1766
RMLVector::VecData
T * VecData
Pointer to the actual vector data, that is, an array of type T objects.
Definition: RMLVector.h:524
RMLOutputParameters::NewPositionVector
RMLDoubleVector * NewPositionVector
A pointer to the new position vector .
Definition: RMLOutputParameters.h:1523
RMLOutputParameters::MaxPosExtremaVelocityVectorArray
RMLDoubleVector ** MaxPosExtremaVelocityVectorArray
A pointer to an array of pointers to RMLDoubleVector objects. The number of array elements equals the...
Definition: RMLOutputParameters.h:1746
RMLOutputParameters::MinPosExtremaVelocityVectorArray
RMLDoubleVector ** MinPosExtremaVelocityVectorArray
A pointer to an array of pointers to RMLDoubleVector objects. The number of array elements equals the...
Definition: RMLOutputParameters.h:1686
TypeIIRMLMath.h
Header file for functions and definitions of constant values and macros.
RMLPositionInputParameters.h
Header file for the class RMLPositionInputParameters.
RMLOutputParameters::MaxExtremaTimesVector
RMLDoubleVector * MaxExtremaTimesVector
A pointer to an RMLDoubleVector object that contains the times at which each degree of freedom reache...
Definition: RMLOutputParameters.h:1587
TypeIIRMLPosition.h
Header file for the class TypeIIRMLPosition, which constitutes the actual interface of the Type II Re...
RMLOutputParameters::MinExtremaTimesVector
RMLDoubleVector * MinExtremaTimesVector
A pointer to an RMLDoubleVector object that contains the times at which each degree of freedom reache...
Definition: RMLOutputParameters.h:1571
TypeIIRMLPolynomial.h
Header file for the class TypeIIRMLMath::TypeIIRMLPolynomial and the struct TypeIIRMLMath::MotionPoly...
RMLPositionOutputParameters
Class for the output parameters of the position-based On-Line Trajectory Generation algorithm...
Definition: RMLPositionOutputParameters.h:73
RMLOutputParameters::MinPosExtremaPositionVectorOnly
RMLDoubleVector * MinPosExtremaPositionVectorOnly
A pointer to an RMLDoubleVector object that contains the maximum positions for all degrees of freedom...
Definition: RMLOutputParameters.h:1603
RMLOutputParameters::MaxPosExtremaPositionVectorOnly
RMLDoubleVector * MaxPosExtremaPositionVectorOnly
A pointer to an RMLDoubleVector object that contains the maximum positions for all degrees of freedom...
Definition: RMLOutputParameters.h:1619
RMLOutputParameters::MinPosExtremaAccelerationVectorArray
RMLDoubleVector ** MinPosExtremaAccelerationVectorArray
A pointer to an array of pointers to RMLDoubleVector objects. The number of array elements equals the...
Definition: RMLOutputParameters.h:1706
Sign
#define Sign(A)
Sign macro (integer)
Definition: TypeIIRMLMath.h:243
RMLOutputParameters::MinPosExtremaPositionVectorArray
RMLDoubleVector ** MinPosExtremaPositionVectorArray
A pointer to an array of pointers to RMLDoubleVector objects. The number of array elements equals the...
Definition: RMLOutputParameters.h:1666
RMLOutputParameters::MaxPosExtremaPositionVectorArray
RMLDoubleVector ** MaxPosExtremaPositionVectorArray
A pointer to an array of pointers to RMLDoubleVector objects. The number of array elements equals the...
Definition: RMLOutputParameters.h:1726
TypeIIRMLPosition::SetPositionalExtremsToZero
void SetPositionalExtremsToZero(RMLPositionOutputParameters *OP) const
Set all positional extremum parameters of the output values of the algorithm (TypeIIRMLPosition::Outp...
Definition: TypeIIRMLCalculatePositionalExtrems.cpp:293

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