TypeIIRMLVelocityCalculatePositionalExtrems.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 <RMLVelocityInputParameters.h>
47 #include <RMLVelocityOutputParameters.h>
48 #include <RMLVector.h>
49 #include <ReflexxesAPI.h>
50 
51 
52 using namespace TypeIIRMLMath;
53 
54 
55 //*******************************************************************************************
56 // CalculatePositionalExtrems()
57 
58 void TypeIIRMLVelocity::CalculatePositionalExtrems( const double &TimeValueInSeconds
59  , RMLVelocityOutputParameters *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->CurrentInputParameters->SelectionVector->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  for (j = 0; j < ((this->Polynomials)[i].ValidPolynomials - 1); j++)
84  {
85  if ((this->Polynomials)[i].PolynomialTimes[j] > TimeValueInSeconds)
86  {
87  if ( Sign( (j == 0)
88  ? ((this->Polynomials)[i].VelocityPolynomial[j].CalculateValue(0.0))
89  : ((this->Polynomials)[i].VelocityPolynomial[j].CalculateValue(
90  (this->Polynomials)[i].PolynomialTimes[j - 1])))
91  !=
92  Sign( (this->Polynomials)[i].VelocityPolynomial[j].CalculateValue(
93  (this->Polynomials)[i].PolynomialTimes[j])) )
94  {
95  (this->Polynomials)[i].VelocityPolynomial[j].CalculateRealRoots( &NumberOfRoots
96  , &TimeOfZeroVelocity1
97  , &TimeOfZeroVelocity2);
98 
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  {
111  (OP->MaxPosExtremaPositionVectorOnly->VecData)[i] = AnalizedPosition;
112  (OP->MaxExtremaTimesVector->VecData)[i] = TimeValueAtExtremumPosition;
113  }
114 
115  if (AnalizedPosition < (OP->MinPosExtremaPositionVectorOnly->VecData)[i])
116  {
117  (OP->MinPosExtremaPositionVectorOnly->VecData)[i] = AnalizedPosition;
118  (OP->MinExtremaTimesVector->VecData)[i] = TimeValueAtExtremumPosition;
119  }
120  }
121  }
122  }
123 
124  AnalizedPosition
125  = (this->Polynomials)[i].PositionPolynomial[
126  ((this->Polynomials)[i].ValidPolynomials - 1)].CalculateValue(
127  (this->Polynomials)[i].PolynomialTimes[
128  ((this->Polynomials)[i].ValidPolynomials - 2)]);
129 
130  TimeValueAtExtremumPosition
131  = (this->Polynomials)[i].PolynomialTimes[((this->Polynomials)[i].ValidPolynomials - 2)];
132 
133  if ((this->Polynomials)[i].PolynomialTimes[(this->Polynomials)[i].ValidPolynomials - 1] > TimeValueInSeconds)
134  {
135  if ( (AnalizedPosition < (OP->MinPosExtremaPositionVectorOnly->VecData)[i])
136  && (TimeValueInSeconds
137  <= (this->Polynomials)[i].PolynomialTimes[((this->Polynomials)[i].ValidPolynomials - 2)]))
138  {
139  (OP->MinPosExtremaPositionVectorOnly->VecData)[i] = AnalizedPosition;
140  (OP->MinExtremaTimesVector->VecData)[i] = TimeValueAtExtremumPosition;
141  }
142 
143  if ( (AnalizedPosition > (OP->MaxPosExtremaPositionVectorOnly->VecData)[i])
144  && (TimeValueInSeconds
145  <= (this->Polynomials)[i].PolynomialTimes[((this->Polynomials)[i].ValidPolynomials - 2)]))
146  {
147  (OP->MaxPosExtremaPositionVectorOnly->VecData)[i] = AnalizedPosition;
148  (OP->MaxExtremaTimesVector->VecData)[i] = TimeValueAtExtremumPosition;
149  }
150  }
151 
152  for (k = 0; k < this->NumberOfDOFs; k++)
153  {
154  if ((this->CurrentInputParameters->SelectionVector->VecData)[k])
155  {
156  for (l = 0; l < MAXIMAL_NO_OF_POLYNOMIALS; l++)
157  {
158  if ( (this->Polynomials)[k].PolynomialTimes[l]
159  >=
160  (OP->MinExtremaTimesVector->VecData)[i])
161  {
162  break;
163  }
164  }
165 
166  (((OP->MinPosExtremaPositionVectorArray)[i])->VecData)[k]
167  = (this->Polynomials)[k].PositionPolynomial[l].CalculateValue(
168  (OP->MinExtremaTimesVector->VecData)[i]);
169  (((OP->MinPosExtremaVelocityVectorArray)[i])->VecData)[k]
170  = (this->Polynomials)[k].VelocityPolynomial[l].CalculateValue(
171  (OP->MinExtremaTimesVector->VecData)[i]);
172  (((OP->MinPosExtremaAccelerationVectorArray)[i])->VecData)[k]
173  = (this->Polynomials)[k].AccelerationPolynomial[l].CalculateValue(
174  (OP->MinExtremaTimesVector->VecData)[i]);
175 
176  for (l = 0; l < MAXIMAL_NO_OF_POLYNOMIALS; l++)
177  {
178  if ( (this->Polynomials)[k].PolynomialTimes[l]
179  >=
180  (OP->MaxExtremaTimesVector->VecData)[i])
181  {
182  break;
183  }
184  }
185 
186  (((OP->MaxPosExtremaPositionVectorArray)[i])->VecData)[k]
187  = (this->Polynomials)[k].PositionPolynomial[l].CalculateValue(
188  (OP->MaxExtremaTimesVector->VecData)[i]);
189  (((OP->MaxPosExtremaVelocityVectorArray)[i])->VecData)[k]
190  = (this->Polynomials)[k].VelocityPolynomial[l].CalculateValue(
191  (OP->MaxExtremaTimesVector->VecData)[i]);
192  (((OP->MaxPosExtremaAccelerationVectorArray)[i])->VecData)[k]
193  = (this->Polynomials)[k].AccelerationPolynomial[l].CalculateValue(
194  (OP->MaxExtremaTimesVector->VecData)[i]);
195  }
196  else
197  {
198  (((OP->MinPosExtremaPositionVectorArray)[i])->VecData)[k]
199  = (this->CurrentInputParameters->CurrentPositionVector->VecData)[k];
200  (((OP->MinPosExtremaVelocityVectorArray)[i])->VecData)[k]
201  = (this->CurrentInputParameters->CurrentVelocityVector->VecData)[k];
202  (((OP->MinPosExtremaAccelerationVectorArray)[i])->VecData)[k]
203  = (this->CurrentInputParameters->CurrentAccelerationVector->VecData)[k];
204 
205  (((OP->MaxPosExtremaPositionVectorArray)[i])->VecData)[k]
206  = (this->CurrentInputParameters->CurrentPositionVector->VecData)[k];
207  (((OP->MaxPosExtremaVelocityVectorArray)[i])->VecData)[k]
208  = (this->CurrentInputParameters->CurrentVelocityVector->VecData)[k];
209  (((OP->MaxPosExtremaAccelerationVectorArray)[i])->VecData)[k]
210  = (this->CurrentInputParameters->CurrentAccelerationVector->VecData)[k];
211  }
212  }
213 
214  OP->MaxExtremaTimesVector->VecData[i] -= TimeValueInSeconds;
215  if (OP->MaxExtremaTimesVector->VecData[i] < 0.0)
216  {
217  OP->MaxExtremaTimesVector->VecData[i] = 0.0;
218  }
219 
220  OP->MinExtremaTimesVector->VecData[i] -= TimeValueInSeconds;
221  if (OP->MinExtremaTimesVector->VecData[i] < 0.0)
222  {
223  OP->MinExtremaTimesVector->VecData[i] = 0.0;
224  }
225  }
226  else
227  {
228  (OP->MinPosExtremaPositionVectorOnly->VecData)[i] = (this->CurrentInputParameters->CurrentPositionVector->VecData)[i];
229  (OP->MaxPosExtremaPositionVectorOnly->VecData)[i] = (this->CurrentInputParameters->CurrentPositionVector->VecData)[i];
230  (OP->MinExtremaTimesVector->VecData)[i] = 0.0;
231  (OP->MaxExtremaTimesVector->VecData)[i] = 0.0;
232 
233  for (k = 0; k < this->NumberOfDOFs; k++)
234  {
235  (((OP->MinPosExtremaPositionVectorArray)[i])->VecData)[k]
236  = (this->CurrentInputParameters->CurrentPositionVector->VecData)[k];
237  (((OP->MinPosExtremaVelocityVectorArray)[i])->VecData)[k]
238  = (this->CurrentInputParameters->CurrentVelocityVector->VecData)[k];
239  (((OP->MinPosExtremaAccelerationVectorArray)[i])->VecData)[k]
240  = (this->CurrentInputParameters->CurrentAccelerationVector->VecData)[k];
241 
242  (((OP->MaxPosExtremaPositionVectorArray)[i])->VecData)[k]
243  = (this->CurrentInputParameters->CurrentPositionVector->VecData)[k];
244  (((OP->MaxPosExtremaVelocityVectorArray)[i])->VecData)[k]
245  = (this->CurrentInputParameters->CurrentVelocityVector->VecData)[k];
246  (((OP->MaxPosExtremaAccelerationVectorArray)[i])->VecData)[k]
247  = (this->CurrentInputParameters->CurrentAccelerationVector->VecData)[k];
248  }
249  }
250  }
251 
252  return;
253 }
254 
255 
256 
257 
258 //*******************************************************************************************
259 // SetPositionalExtremsToZero()
260 
261 void TypeIIRMLVelocity::SetPositionalExtremsToZero(RMLVelocityOutputParameters *OP) const
262 {
263 
264  unsigned int i = 0
265  , k = 0;
266 
267  for (i = 0; i < this->NumberOfDOFs; i++)
268  {
269  for (k = 0; k < this->NumberOfDOFs; k++)
270  {
271  (((OP->MinPosExtremaPositionVectorArray)[i])->VecData)[k]
272  = 0.0;
273  (((OP->MinPosExtremaVelocityVectorArray)[i])->VecData)[k]
274  = 0.0;
275  (((OP->MinPosExtremaAccelerationVectorArray)[i])->VecData)[k]
276  = 0.0;
277 
278  (((OP->MaxPosExtremaPositionVectorArray)[i])->VecData)[k]
279  = 0.0;
280  (((OP->MaxPosExtremaVelocityVectorArray)[i])->VecData)[k]
281  = 0.0;
282  (((OP->MaxPosExtremaAccelerationVectorArray)[i])->VecData)[k]
283  = 0.0;
284  }
285 
286  (OP->MinPosExtremaPositionVectorOnly->VecData)[i]
287  = 0.0;
288  (OP->MaxPosExtremaPositionVectorOnly->VecData)[i]
289  = 0.0;
290  (OP->MinExtremaTimesVector->VecData)[i]
291  = 0.0;
292  (OP->MaxExtremaTimesVector->VecData)[i]
293  = 0.0;
294  }
295 
296 }
RMLVector.h
Header file for the dynamic vector class used for the Reflexxes Motion Libraries. ...
ReflexxesAPI.h
Header file for the class ReflexxesAPI (API of the Reflexxes Motion Libraries)
TypeIIRMLVelocity::SetPositionalExtremsToZero
void SetPositionalExtremsToZero(RMLVelocityOutputParameters *OP) const
Set all positional extremum parameters of the output values of the algorithm (TypeIIRMLVelocity::Outp...
Definition: TypeIIRMLVelocityCalculatePositionalExtrems.cpp:261
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.
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.
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
RMLVelocityInputParameters.h
Header file for the class RMLVelocityInputParameters.
TypeIIRMLVelocity::CalculatePositionalExtrems
void CalculatePositionalExtrems(const double &TimeValueInSeconds, RMLVelocityOutputParameters *OP) const
Set all positional extremum parameters of the output values of the algorithm (TypeIIRMLVelocity::Outp...
Definition: TypeIIRMLVelocityCalculatePositionalExtrems.cpp:58
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
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
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

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