TypeIIRMLIsPhaseSynchronizationPossible.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 <TypeIIRMLStep1IntermediateProfiles.h>
47 #include <TypeIIRMLStep1Profiles.h>
48 #include <RMLPositionInputParameters.h>
49 
50 using namespace TypeIIRMLMath;
51 
52 
53 //*******************************************************************************************
54 // IsPhaseSynchronizationPossible
55 
56 bool TypeIIRMLPosition::IsPhaseSynchronizationPossible(RMLDoubleVector* ReferenceVector)
57 {
58  bool Result = true
59  , SignSwitch = false;
60 
61  unsigned int i = 0;
62 
63  double LengthOfDirectionVector = 0.0
64  , LengthOfCurrentVelocityVector = 0.0
65  , LengthOfTargetVelocityVector = 0.0
66  , LengthOfReferenceVector = 0.0;
67 
68 
69  for (i = 0; i < this->NumberOfDOFs; i++)
70  {
71  if ((this->ModifiedSelectionVector->VecData)[i])
72  {
73  (this->PhaseSynchronizationPositionDifferenceVector->VecData)[i]
74  = (this->CurrentInputParameters->TargetPositionVector->VecData)[i]
75  - (this->CurrentInputParameters->CurrentPositionVector->VecData)[i];
76  (this->PhaseSynchronizationCurrentVelocityVector->VecData)[i]
77  = (this->CurrentInputParameters->CurrentVelocityVector->VecData)[i];
78  (this->PhaseSynchronizationTargetVelocityVector->VecData)[i]
79  = (this->CurrentInputParameters->TargetVelocityVector->VecData)[i];
80 
81  LengthOfDirectionVector += pow2((this->PhaseSynchronizationPositionDifferenceVector->VecData)[i]);
82  LengthOfCurrentVelocityVector += pow2((this->PhaseSynchronizationCurrentVelocityVector->VecData)[i]);
83  LengthOfTargetVelocityVector += pow2((this->PhaseSynchronizationTargetVelocityVector->VecData)[i]);
84  }
85  else
86  {
87  (this->PhaseSynchronizationPositionDifferenceVector->VecData) [i] = 0.0;
88  (this->PhaseSynchronizationCurrentVelocityVector->VecData) [i] = 0.0;
89  (this->PhaseSynchronizationTargetVelocityVector->VecData) [i] = 0.0;
90  }
91  }
92 
93  LengthOfDirectionVector = RMLSqrt(LengthOfDirectionVector);
94  LengthOfCurrentVelocityVector = RMLSqrt(LengthOfCurrentVelocityVector);
95  LengthOfTargetVelocityVector = RMLSqrt(LengthOfTargetVelocityVector);
96 
97 
98  if ( (LengthOfDirectionVector != POSITIVE_ZERO) && (LengthOfDirectionVector != 0.0) )
99  {
100  for (i = 0; i < this->NumberOfDOFs; i++)
101  {
102  if ((this->ModifiedSelectionVector->VecData)[i])
103  {
104  (this->PhaseSynchronizationPositionDifferenceVector->VecData)[i] /= LengthOfDirectionVector;
105  }
106  }
107  }
108  else
109  {
110  for (i = 0; i < this->NumberOfDOFs; i++)
111  {
112  if ((this->ModifiedSelectionVector->VecData)[i])
113  {
114  (this->PhaseSynchronizationPositionDifferenceVector->VecData)[i] = 0.0;
115  }
116  }
117  }
118 
119  if ( (LengthOfCurrentVelocityVector != POSITIVE_ZERO) && (LengthOfCurrentVelocityVector != 0.0) )
120  {
121  for (i = 0; i < this->NumberOfDOFs; i++)
122  {
123  if ((this->ModifiedSelectionVector->VecData)[i])
124  {
125  (this->PhaseSynchronizationCurrentVelocityVector->VecData)[i] /= LengthOfCurrentVelocityVector;
126  }
127  }
128  }
129  else
130  {
131  for (i = 0; i < this->NumberOfDOFs; i++)
132  {
133  if ((this->ModifiedSelectionVector->VecData)[i])
134  {
135  (this->PhaseSynchronizationCurrentVelocityVector->VecData)[i] = 0.0;
136  }
137  }
138  }
139 
140  if ( (LengthOfTargetVelocityVector != POSITIVE_ZERO) && (LengthOfTargetVelocityVector != 0.0) )
141  {
142  for (i = 0; i < this->NumberOfDOFs; i++)
143  {
144  if ((this->ModifiedSelectionVector->VecData)[i])
145  {
146  (this->PhaseSynchronizationTargetVelocityVector->VecData)[i] /= LengthOfTargetVelocityVector;
147  }
148  }
149  }
150  else
151  {
152  for (i = 0; i < this->NumberOfDOFs; i++)
153  {
154  if ((this->ModifiedSelectionVector->VecData)[i])
155  {
156  (this->PhaseSynchronizationTargetVelocityVector->VecData)[i] = 0.0;
157  }
158  }
159  }
160 
161  // Determine a reference vector.
162 
163  LengthOfReferenceVector = ABSOLUTE_PHASE_SYNC_EPSILON;
164 
165  if ( ( LengthOfDirectionVector >= LengthOfCurrentVelocityVector )
166  && ( LengthOfDirectionVector >= LengthOfTargetVelocityVector )
167  && ( LengthOfDirectionVector >= ABSOLUTE_PHASE_SYNC_EPSILON ))
168  {
169  *(this->PhaseSynchronizationCheckVector) = *(this->PhaseSynchronizationPositionDifferenceVector);
170  LengthOfReferenceVector = LengthOfDirectionVector;
171  this->PhaseSynchronizationMagnitude = TypeIIRMLPosition::POSITION;
172  }
173 
174  if ( ( LengthOfCurrentVelocityVector >= LengthOfDirectionVector )
175  && ( LengthOfCurrentVelocityVector >= LengthOfTargetVelocityVector )
176  && ( LengthOfCurrentVelocityVector >= ABSOLUTE_PHASE_SYNC_EPSILON ))
177  {
178  *(this->PhaseSynchronizationCheckVector) = *(this->PhaseSynchronizationCurrentVelocityVector);
179  LengthOfReferenceVector = LengthOfCurrentVelocityVector;
180  this->PhaseSynchronizationMagnitude = TypeIIRMLPosition::CURRENT_VELOCITY;
181  }
182 
183  if ( ( LengthOfTargetVelocityVector >= LengthOfDirectionVector )
184  && ( LengthOfTargetVelocityVector >= LengthOfCurrentVelocityVector )
185  && ( LengthOfTargetVelocityVector >= ABSOLUTE_PHASE_SYNC_EPSILON ))
186  {
187  *(this->PhaseSynchronizationCheckVector) = *(this->PhaseSynchronizationTargetVelocityVector);
188  LengthOfReferenceVector = LengthOfTargetVelocityVector;
189  this->PhaseSynchronizationMagnitude = TypeIIRMLPosition::TARGET_VELOCITY;
190  }
191 
192  if (LengthOfReferenceVector > ABSOLUTE_PHASE_SYNC_EPSILON)
193  {
194  // Switch vector orientations
195 
196  SignSwitch = true;
197 
198  for (i = 0; i < this->NumberOfDOFs; i++)
199  {
200  if ((this->ModifiedSelectionVector->VecData)[i])
201  {
202  if ((Sign((this->PhaseSynchronizationPositionDifferenceVector->VecData)[i])
203  == Sign((this->PhaseSynchronizationCheckVector->VecData)[i]))
204  && (fabs((this->PhaseSynchronizationPositionDifferenceVector->VecData)[i])
205  > ABSOLUTE_PHASE_SYNC_EPSILON))
206  {
207  SignSwitch = false;
208  break;
209  }
210  }
211  }
212 
213  if (SignSwitch)
214  {
215  for (i = 0; i < this->NumberOfDOFs; i++)
216  {
217  if ((this->ModifiedSelectionVector->VecData)[i])
218  {
219  (this->PhaseSynchronizationPositionDifferenceVector->VecData)[i]
220  = -(this->PhaseSynchronizationPositionDifferenceVector->VecData)[i];
221  }
222  }
223  }
224 
225  SignSwitch = true;
226 
227  for (i = 0; i < this->NumberOfDOFs; i++)
228  {
229  if ((this->ModifiedSelectionVector->VecData)[i])
230  {
231  if ((Sign((this->PhaseSynchronizationCurrentVelocityVector->VecData)[i])
232  == Sign((this->PhaseSynchronizationCheckVector->VecData)[i]))
233  && (fabs((this->PhaseSynchronizationCurrentVelocityVector->VecData)[i])
234  > ABSOLUTE_PHASE_SYNC_EPSILON))
235  {
236  SignSwitch = false;
237  break;
238  }
239  }
240  }
241 
242  if (SignSwitch)
243  {
244  for (i = 0; i < this->NumberOfDOFs; i++)
245  {
246  if ((this->ModifiedSelectionVector->VecData)[i])
247  {
248  (this->PhaseSynchronizationCurrentVelocityVector->VecData)[i]
249  = -(this->PhaseSynchronizationCurrentVelocityVector->VecData)[i];
250  }
251  }
252  }
253 
254  SignSwitch = true;
255 
256  for (i = 0; i < this->NumberOfDOFs; i++)
257  {
258  if ((this->ModifiedSelectionVector->VecData)[i])
259  {
260  if ((Sign((this->PhaseSynchronizationTargetVelocityVector->VecData)[i])
261  == Sign((this->PhaseSynchronizationCheckVector->VecData)[i]))
262  && (fabs((this->PhaseSynchronizationTargetVelocityVector->VecData)[i])
263  > ABSOLUTE_PHASE_SYNC_EPSILON))
264  {
265  SignSwitch = false;
266  break;
267  }
268  }
269  }
270 
271  if (SignSwitch)
272  {
273  for (i = 0; i < this->NumberOfDOFs; i++)
274  {
275  if ((this->ModifiedSelectionVector->VecData)[i])
276  {
277  (this->PhaseSynchronizationTargetVelocityVector->VecData)[i]
278  = -(this->PhaseSynchronizationTargetVelocityVector->VecData)[i];
279  }
280  }
281  }
282 
283  // Check for collinearity
284 
285  for (i = 0; i < this->NumberOfDOFs; i++)
286  {
287  if ( (this->ModifiedSelectionVector->VecData)[i] )
288  {
289  if ( ((fabs((this->PhaseSynchronizationCheckVector->VecData)[i] - (this->PhaseSynchronizationPositionDifferenceVector->VecData)[i])
290  > (PHASE_SYNC_COLLINEARITY_REL_EPSILON * fabs((this->PhaseSynchronizationCheckVector->VecData)[i]) ) )
291  && ( LengthOfDirectionVector >= ABSOLUTE_PHASE_SYNC_EPSILON ))
292  || ((fabs((this->PhaseSynchronizationCheckVector->VecData)[i] - (this->PhaseSynchronizationCurrentVelocityVector->VecData)[i])
293  > (PHASE_SYNC_COLLINEARITY_REL_EPSILON * fabs((this->PhaseSynchronizationCheckVector->VecData)[i]) ) )
294  && ( LengthOfCurrentVelocityVector >= ABSOLUTE_PHASE_SYNC_EPSILON ))
295  || ((fabs((this->PhaseSynchronizationCheckVector->VecData)[i] - (this->PhaseSynchronizationTargetVelocityVector->VecData)[i])
296  > (PHASE_SYNC_COLLINEARITY_REL_EPSILON * fabs((this->PhaseSynchronizationCheckVector->VecData)[i]) ) )
297  && ( LengthOfTargetVelocityVector >= ABSOLUTE_PHASE_SYNC_EPSILON )))
298  {
299  Result = false;
300  break;
301  }
302  }
303  }
304  }
305  else
306  {
307  Result = false;
308  }
309 
310  if (Result)
311  {
312  *ReferenceVector = *(this->PhaseSynchronizationCheckVector);
313  }
314  else
315  {
316  ReferenceVector->Set(0.0);
317  }
318 
319  return(Result);
320 }
ABSOLUTE_PHASE_SYNC_EPSILON
#define ABSOLUTE_PHASE_SYNC_EPSILON
Absolute epsilon to check whether all required vectors are collinear.
Definition: TypeIIRMLMath.h:179
TypeIIRMLPosition::CURRENT_VELOCITY
The current acceleration vector was to the dominant value to determine the reference vector for a p...
Definition: TypeIIRMLPosition.h:384
TypeIIRMLStep1IntermediateProfiles.h
Header file for intermediate profile segments of Step 1.
RMLVector::Set
void Set(const T Value)
Sets all elements of a vector of double elements to one specific value.
Definition: RMLVector.h:384
TypeIIRMLMath.h
Header file for functions and definitions of constant values and macros.
RMLPositionInputParameters.h
Header file for the class RMLPositionInputParameters.
TypeIIRMLMath::RMLSqrt
double RMLSqrt(const double &Value)
Calculates the real square root of a given value. If the value is negative a value of almost zero wil...
Definition: TypeIIRMLMath.h:291
TypeIIRMLPosition.h
Header file for the class TypeIIRMLPosition, which constitutes the actual interface of the Type II Re...
pow2
#define pow2(A)
A to the power of 2.
Definition: TypeIIRMLMath.h:273
TypeIIRMLPosition::TARGET_VELOCITY
The target velocity vector was to the dominant value to determine the reference vector for a phase-...
Definition: TypeIIRMLPosition.h:388
TypeIIRMLPosition::POSITION
The position difference vector was to the dominant value to determine the reference vector for a ph...
Definition: TypeIIRMLPosition.h:380
TypeIIRMLStep1Profiles.h
Header file for the Step 1 motion profiles.
TypeIIRMLPosition::IsPhaseSynchronizationPossible
bool IsPhaseSynchronizationPossible(RMLDoubleVector *ReferenceVector)
Checks, whether the motion trajectory can be phase-synchronized.
Definition: TypeIIRMLIsPhaseSynchronizationPossible.cpp:56
PHASE_SYNC_COLLINEARITY_REL_EPSILON
#define PHASE_SYNC_COLLINEARITY_REL_EPSILON
Relative value to check for collinearity during the check for phase synchronization.
Definition: TypeIIRMLMath.h:228
Sign
#define Sign(A)
Sign macro (integer)
Definition: TypeIIRMLMath.h:243
RMLVector
This is a minimalistic dynamic vector class implementation used for the Reflexxes Motion Libraries...
Definition: RMLVector.h:77
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