InterpolationSegmentFactory.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package ArmarXGuiPlugins::RobotTrajectoryDesigner::Interpolation
17  * @author Timo Birr
18  * @date 2018
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 #include "InterpolationSegmentFactory.h"
23 #include "LinearInterpolation.h"
24 #include "SplineInterpolation.h"
25 #include <ArmarXCore/core/logging/Logging.h>
26 #include <RobotAPI/libraries/core/FramedPose.h>
27 
28 ///EXCEPTIONS
29 #include "../exceptions/InterpolationNotDefinedException.h"
30 #include "../exceptions/NoInterpolationPossibleException.h"
31 #include "../exceptions/ControlPointsInterpolationMatchException.h"
32 
33 using namespace armarx;
34 using namespace VirtualRobot;
35 
36 std::vector<AbstractInterpolationPtr> InterpolationSegmentFactory::produceInterpolationSegments(std::vector<PoseBasePtr> controlPoints, std::vector<InterpolationType> interpolations)
37 {
38  if (controlPoints.size() != interpolations.size() + 1)
39  {
40  throw new exceptions::local::ControlPointsInterpolationMatchException(controlPoints.size(), interpolations.size());
41  }
42 
43  std::vector<AbstractInterpolationPtr> producedInterpolations = std::vector<AbstractInterpolationPtr>();
44 
45  std::vector<int> interPolationChangePoints = std::vector<int>();
46  interPolationChangePoints.push_back(0);
47 
48  if (interpolations.size() == 1 && interpolations.at(0) == eSplineInterpolation)
49  {
50  interpolations.assign(0, eLinearInterpolation);
51  }
52 
53  //mark parts of the Interpolaion where it changes
54  int splineCounter = 0;
55  InterpolationType current = interpolations.at(0);
56  if (eSplineInterpolation == interpolations.at(0))
57  {
58  splineCounter++;
59  }
60  for (unsigned int i = 1; i < interpolations.size(); i++)
61  {
62  if (eSplineInterpolation == interpolations.at(i))
63  {
64  splineCounter++;
65  }
66  if (current != interpolations.at(i))
67  {
68  if (current == eSplineInterpolation && interpolations.at(i) != eSplineInterpolation)
69  {
70  if (splineCounter < 2)
71  {
72  interpolations[i - 1] = eLinearInterpolation;
73  }
74  else
75  {
76  interPolationChangePoints.push_back(i);
77  }
78  splineCounter = 0;
79  }
80  else
81  {
82  interPolationChangePoints.push_back(i);
83  }
84  current = interpolations[i];
85 
86  }
87 
88  }
89  if (interpolations.size() >= 2 && interpolations.at(interpolations.size() - 1) == eSplineInterpolation && interpolations.at(interpolations.size() - 2) != eSplineInterpolation)
90  {
91  interpolations.pop_back();
92  interpolations.push_back(eLinearInterpolation);
93  }
94  //mark parts of the Interpolaion where it changes
95  /*InterpolationType current = interpolations.at(0);
96  for (unsigned int i = 1; i < interpolations.size(); i++)
97  {
98  if (current != interpolations.at(i))
99  {
100  int prev = interPolationChangePoints.back();
101  //A splineInterpolation needs 3 Points or more to be a valid spline Interpolation
102  if (i - prev < 2 && interpolations.at(prev + 1) == InterpolationType::eSplineInterpolation && interpolations.at(i) == InterpolationType::eSplineInterpolation)
103  {
104  throw new exceptions::local::NoInterpolationPossibleException(3, 2);
105  }
106  interPolationChangePoints.push_back(i);
107  current = interpolations[i];
108  }
109  }*/
110  //Produce the InterpolationSegments and add them to producedInterpolationsVector
111  for (unsigned int j = 1; j < interPolationChangePoints.size(); j++)
112  {
113  std::vector<PoseBasePtr> currentCP = std::vector<PoseBasePtr>();
114  for (int i = interPolationChangePoints.at(j - 1); i <= interPolationChangePoints.at(j); i++)
115  {
116  currentCP.push_back(controlPoints.at(i));
117  }
118  std::vector<AbstractInterpolationPtr> temp;
119  switch (interpolations.at(interPolationChangePoints.at(j - 1)))
120  {
121  case (InterpolationType::eLinearInterpolation):
122  temp = produceLinearInterpolationSegments(currentCP);
123  break;
124  case (InterpolationType::eSplineInterpolation):
125  temp = produceSplineInterpolationSegments(currentCP);
126  break;
127  }
128  producedInterpolations.insert(producedInterpolations.end(), temp.begin(), temp.end());
129  }
130 
131  std::vector<PoseBasePtr> currentCP = std::vector<PoseBasePtr>();
132  for (unsigned int i = interPolationChangePoints.at(interPolationChangePoints.size() - 1); i < controlPoints.size(); i++)
133  {
134  currentCP.push_back(controlPoints.at(i));
135  }
136  std::vector<AbstractInterpolationPtr> temp;
137  switch (interpolations.at(interpolations.size() - 1))
138  {
139  case (InterpolationType::eLinearInterpolation):
140  temp = produceLinearInterpolationSegments(currentCP);
141  break;
142  case (InterpolationType::eSplineInterpolation):
143  temp = produceSplineInterpolationSegments(currentCP);
144  break;
145  }
146  producedInterpolations.insert(producedInterpolations.end(), temp.begin(), temp.end());
147  return producedInterpolations;
148 }
149 
150 
151 std::vector<AbstractInterpolationPtr> InterpolationSegmentFactory::produceLinearInterpolationSegments(std::vector<PoseBasePtr> controlPoints)
152 {
153  std::vector<AbstractInterpolationPtr> producedInterpolations = std::vector<AbstractInterpolationPtr>();
154  for (unsigned int i = 0; i < controlPoints.size() - 1; i++)
155  {
156  std::vector<PoseBasePtr> currentCP = std::vector<PoseBasePtr>();
157  currentCP.push_back(controlPoints.at(i));
158  currentCP.push_back(controlPoints.at(i + 1));
159  AbstractInterpolationPtr interpolation = std::shared_ptr<AbstractInterpolation>(new LinearInterpolation(currentCP));
160  producedInterpolations.push_back(interpolation);
161  }
162  return producedInterpolations;
163 }
164 
165 std::vector<AbstractInterpolationPtr> InterpolationSegmentFactory::produceSplineInterpolationSegments(std::vector<PoseBasePtr> controlPoints)
166 {
167  std::vector<AbstractInterpolationPtr> producedInterpolations = std::vector<AbstractInterpolationPtr>();
168  SplineInterpolationPtr parent = *new std::shared_ptr<SplineInterpolation>(new SplineInterpolation(controlPoints));//das hier muss genauso aussehen
169 
170  for (unsigned int i = 0; i < controlPoints.size() - 1; i++)
171  {
172  AbstractInterpolationPtr interpolation = std::shared_ptr<AbstractInterpolation>(parent->getInterPolationSegment(i));
173  producedInterpolations.push_back(interpolation);
174  }
175  return producedInterpolations;
176 }
177 
178 AbstractInterpolationPtr InterpolationSegmentFactory::produceSplineInterpolationSegment(std::vector<PoseBasePtr> controlPoints, PoseBasePtr segmentStart)
179 {
180  SplineInterpolationPtr parent = *new std::shared_ptr<SplineInterpolation>(new SplineInterpolation(controlPoints));//das hier muss genauso aussehen
181 
182  AbstractInterpolationPtr interpolation = std::shared_ptr<AbstractInterpolation>(parent->getInterPolationSegment(segmentStart));
183 
184  return interpolation;
185 }
186 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
187 /// \brief InterpolationSegmentFactory::optimizeControlPoints
188 /// \param controlPoints
189 /// \param selections
190 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
191 void InterpolationSegmentFactory::optimizeControlPoints(std::vector<PoseBasePtr>& controlPoints, std::vector<IKSolver::CartesianSelection>& selections)
192 {
193  /*for(PoseBasePtr pose: controlPoints) {
194  ARMARX_WARNING << Pose(pose->position, pose->orientation).toEigen();
195  }
196  for(IKSolver::CartesianSelection sel: selections) {
197  ARMARX_WARNING << std::to_string(sel);
198  }*/
199 
200  for (unsigned int i = 0; i < selections.size() - 1; i++)
201  {
202  //ARMARX_INFO << "OPTIMIZE";
203  if (InterpolationSegmentFactory::isDominantOver(selections[i + 1], selections[i]))
204  {
205  std::vector<IKSolver::CartesianSelection> followingDominations = std::vector<IKSolver::CartesianSelection>();
206  followingDominations.push_back(selections[i + 1]);
207  int followingDominationsStart = i;
208  bool isBreaking;
209  do
210  {
211  //look left until an equal or fully dominating cartesian selection appears
212  for (int j = i ; j >= 0; j--)
213  {
214  if ((InterpolationSegmentFactory::isDominantOver(selections[j], selections[i + 1]) && !InterpolationSegmentFactory::isDominantOver(selections[i + 1], selections[j]))
215  || selections[i + 1] == selections[j])
216  {
217 
218  //ARMARX_INFO << std::to_string(selections[j]) + " is dominant over " + std::to_string(selections[i + 1]);
219  followingDominationsStart = j + 1;
220 
221  int dominanceIntervalSize = i + 1 - j;
222  PoseBasePtr dominanceIntervalStart = controlPoints[j];
223  PoseBasePtr dominanceIntervalEnd = controlPoints[i + 1];
224  std::vector<AbstractInterpolationPtr> li = produceLinearInterpolationSegments({dominanceIntervalStart, dominanceIntervalEnd});
225  int counter = 1;
226  for (unsigned int k = j + 1; k < i + 1; k++)
227  {
228  controlPoints[k] = optimizePose(controlPoints[k], li[0]->getPoseAt(double(1.0 / dominanceIntervalSize) * counter), selections[k], selections[i + 1]);
229  counter++;
230  //ARMARX_INFO << std::to_string(double(1.0 / dominanceIntervalSize) * k) + " SET POINT AT------" + std::to_string(k) + " with " + std::to_string(selections[i + 1]);
231  }
232  break;
233 
234  }
235  followingDominations.push_back(selections[j]);
236  }
237 
238  if (i + 2 < selections.size() && isDominantOver(selections[i + 2], selections[i + 1]))
239  {
240  isBreaking = false;
241  i++;
242  followingDominations.push_back(selections[i + 1]);
243  }
244  else
245  {
246  isBreaking = true;
247  }
248 
249  }
250  while (!isBreaking);
251  /*ARMARX_WARNING << "FOLLOWING DOMINATIONS START ";
252  for(IKSolver::CartesianSelection sel: followingDominations) {
253  ARMARX_WARNING << std::to_string(sel);
254  }
255  ARMARX_WARNING << "FOLLOWING DOMINATIONS END ";*/
256  IKSolver::CartesianSelection dominatingSelection = getSmallestDominating(followingDominations);
257  for (unsigned int m = followingDominationsStart; m <= i + 1; m++)
258  {
259  //ARMARX_WARNING << "SET SELECTION " + std::to_string(m) + " with " + std::to_string(dominatingSelection);
260  selections[m] = dominatingSelection;
261  //selections[m] = IKSolver::CartesianSelection::All;
262  }
263  }
264  //ARMARX_INFO << "Optimizing done";
265 
266  }
267  ARMARX_INFO << "Optimizing done";
268 
269  /*ARMARX_WARNING << "RESULT XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX";
270  for(PoseBasePtr pose: controlPoints) {
271  ARMARX_WARNING << Pose(pose->position, pose->orientation).toEigen();
272  }
273  for(IKSolver::CartesianSelection sel: selections) {
274  ARMARX_WARNING << std::to_string(sel);
275  }*/
276 }
277 
278 bool InterpolationSegmentFactory::needsOptimizing(std::vector<IKSolver::CartesianSelection>& selections)
279 {
280  for (unsigned int i = 0; i < selections.size() - 1; i++)
281  {
282  //ARMARX_INFO << "NEEDS OPTIMIZING";
283  if (InterpolationSegmentFactory::isDominantOver(selections[i + 1], selections[i]))
284  {
285  return true;
286  }
287  }
288  return false;
289 }
290 
291 bool InterpolationSegmentFactory::isDominantOver(VirtualRobot::IKSolver::CartesianSelection current, VirtualRobot::IKSolver::CartesianSelection other)
292 {
293  if (other == IKSolver::CartesianSelection::X || other == IKSolver::CartesianSelection::Y || other == IKSolver::CartesianSelection::Z || other == IKSolver::CartesianSelection::Orientation)
294  {
295  return current != other;
296  }
297  else if (other == IKSolver::CartesianSelection::Position)
298  {
299  return (current == IKSolver::CartesianSelection::Orientation || current == IKSolver::CartesianSelection::All);
300  }
301  return false;
302 }
303 
304 IKSolver::CartesianSelection InterpolationSegmentFactory::getSmallestDominating(std::vector<VirtualRobot::IKSolver::CartesianSelection> selections)
305 {
306  bool isInhomogenous = false;
307  ARMARX_INFO << "Vector start";
308  int max = 0;
309  for (IKSolver::CartesianSelection current : selections)
310  {
311  if (current > max)
312  {
313  ARMARX_INFO << "In Vector" + std::to_string(current);
314  max = current;
315  }
316  if (max != 0 && current != max)
317  {
318  isInhomogenous = true;
319  }
320  }
321  if (max <= 4 && isInhomogenous)
322  {
323  ARMARX_INFO << "SMALLEST DOMINATING" + std::to_string(IKSolver::CartesianSelection::Position);
324  return IKSolver::CartesianSelection::Position;
325  }
326  else if (max == IKSolver::CartesianSelection::Orientation)
327  {
328  for (IKSolver::CartesianSelection currentSelection : selections)
329  {
330  if (currentSelection != IKSolver::CartesianSelection::Orientation)
331  {
332  ARMARX_INFO << "SMALLEST DOMINATING" + std::to_string(IKSolver::CartesianSelection::All);
333  return IKSolver::CartesianSelection::All;
334  }
335  }
336  ARMARX_INFO << "SMALLEST DOMINATING" + std::to_string(IKSolver::CartesianSelection::Orientation);
337  return IKSolver::CartesianSelection::Orientation;
338  }
339  else
340  {
341  ARMARX_INFO << "SMALLEST DOMINATING" + std::to_string(max);
342  return static_cast<IKSolver::CartesianSelection>(max);
343  }
344 
345 }
346 
347 PoseBasePtr InterpolationSegmentFactory::optimizePose(PoseBasePtr original, PoseBasePtr corrected, IKSolver::CartesianSelection selectionOriginal, IKSolver::CartesianSelection selectionCorrecting)
348 {
349  //ARMARX_WARNING << Pose(original->position, original->orientation).toEigen();
350  //ARMARX_WARNING << Pose(corrected->position, corrected->orientation).toEigen();
351  PoseBasePtr firstStep;
352  switch (selectionCorrecting)
353  {
354  case (IKSolver::CartesianSelection::X):
355  firstStep = PoseBasePtr(new Pose(Vector3BasePtr(new Vector3(corrected->position->x, original->position->y, original->position->z)), original->orientation));
356  case (IKSolver::CartesianSelection::Y):
357  firstStep = PoseBasePtr(new Pose(Vector3BasePtr(new Vector3(original->position->x, corrected->position->y, original->position->z)), original->orientation));
358  case (IKSolver::CartesianSelection::Z):
359  firstStep = PoseBasePtr(new Pose(Vector3BasePtr(new Vector3(original->position->x, original->position->y, corrected->position->z)), original->orientation));
360  case (IKSolver::CartesianSelection::Position):
361  firstStep = PoseBasePtr(new Pose(corrected->position, original->orientation));
362  case (IKSolver::CartesianSelection::Orientation):
363  firstStep = PoseBasePtr(new Pose(original->position, corrected->orientation));
364  case (IKSolver::CartesianSelection::All):
365  //ARMARX_WARNING << "CORRECTED ALL";
366  firstStep = corrected;
367  }
368  switch (selectionOriginal)
369  {
370  case (IKSolver::CartesianSelection::X):
371  return PoseBasePtr(new Pose(Vector3BasePtr(new Vector3(original->position->x, firstStep->position->y, firstStep->position->z)), firstStep->orientation));
372  case (IKSolver::CartesianSelection::Y):
373  return PoseBasePtr(new Pose(Vector3BasePtr(new Vector3(firstStep->position->x, original->position->y, firstStep->position->z)), firstStep->orientation));
374  case (IKSolver::CartesianSelection::Z):
375  return PoseBasePtr(new Pose(Vector3BasePtr(new Vector3(firstStep->position->x, firstStep->position->y, original->position->z)), firstStep->orientation));
376  case (IKSolver::CartesianSelection::Position):
377  return PoseBasePtr(new Pose(original->position, firstStep->orientation));
378  case (IKSolver::CartesianSelection::Orientation):
379  return PoseBasePtr(new Pose(firstStep->position, original->orientation));
380  case (IKSolver::CartesianSelection::All):
381  return original;
382  default:
383  return corrected;
384  }
385 }
386 
387 
388 
GfxTL::Orientation
ScalarT Orientation(const VectorXD< 2, ScalarT > &p1, const VectorXD< 2, ScalarT > &p2, const VectorXD< 2, ScalarT > &c)
Definition: Orientation.h:9
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
VirtualRobot
Definition: FramedPose.h:43
armarx::InterpolationSegmentFactory::optimizePose
static PoseBasePtr optimizePose(armarx::PoseBasePtr original, armarx::PoseBasePtr corrected, VirtualRobot::IKSolver::CartesianSelection selectionOriginal, VirtualRobot::IKSolver::CartesianSelection selectionCorrected)
optimizePose changes the original pose so that the interpolation is smove when it changes from select...
Definition: InterpolationSegmentFactory.cpp:347
armarx::max
std::vector< T > max(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:267
armarx::InterpolationSegmentFactory::needsOptimizing
static bool needsOptimizing(std::vector< VirtualRobot::IKSolver::CartesianSelection > &selections)
needsOptimizing returns true if there is a CartesianSelection at i that dominates a CartesianSelectio...
Definition: InterpolationSegmentFactory.cpp:278
armarx::eSplineInterpolation
@ eSplineInterpolation
Definition: InterpolationType.h:35
armarx::AbstractInterpolationPtr
std::shared_ptr< AbstractInterpolation > AbstractInterpolationPtr
Definition: AbstractInterpolation.h:77
armarx::SplineInterpolationPtr
std::shared_ptr< SplineInterpolation > SplineInterpolationPtr
Definition: SplineInterpolation.h:75
FramedPose.h
armarx::InterpolationSegmentFactory::isDominantOver
static bool isDominantOver(VirtualRobot::IKSolver::CartesianSelection current, VirtualRobot::IKSolver::CartesianSelection other)
isDominantOver returns true when current selection dominates other selection Definition: A CartesianS...
Definition: InterpolationSegmentFactory.cpp:291
armarx::InterpolationType
InterpolationType
The InterpolationType enum lists all available interpolation types eLinearInterpolation: represents l...
Definition: InterpolationType.h:32
SplineInterpolation.h
armarx::NJointTaskSpaceDMPControllerMode::CartesianSelection
CartesianSelection
Definition: ControllerInterface.ice:34
InterpolationSegmentFactory.h
armarx::InterpolationSegmentFactory::produceLinearInterpolationSegments
static std::vector< AbstractInterpolationPtr > produceLinearInterpolationSegments(std::vector< PoseBasePtr > controlPoints)
produceInterpolationSegments constructs a vector of LinearInterpolations
Definition: InterpolationSegmentFactory.cpp:151
armarx::InterpolationSegmentFactory::produceSplineInterpolationSegment
static AbstractInterpolationPtr produceSplineInterpolationSegment(std::vector< PoseBasePtr > controlPoints, PoseBasePtr segmentStart)
produceSplineInterpolationSegment constructs the splineInterpolationSegment of a splineInterpolation ...
Definition: InterpolationSegmentFactory.cpp:178
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:40
armarx::LinearInterpolation
The LinearInterpolation class represents a linear Interpolation between a series of control points Li...
Definition: LinearInterpolation.h:36
LinearInterpolation.h
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:41
armarx::InterpolationSegmentFactory::optimizeControlPoints
static void optimizeControlPoints(std::vector< PoseBasePtr > &controlPoints, std::vector< VirtualRobot::IKSolver::CartesianSelection > &selections)
optimizeControlPoints changes the cartian selections and control points so that the IKSolving produce...
Definition: InterpolationSegmentFactory.cpp:191
armarx::InterpolationSegmentFactory::produceInterpolationSegments
static std::vector< AbstractInterpolationPtr > produceInterpolationSegments(std::vector< PoseBasePtr > controlPoints, std::vector< InterpolationType > interpolations)
produceInterpolationSegments constructs a vector of AbstractInterpolation the concrete Interpolationt...
Definition: InterpolationSegmentFactory.cpp:36
Logging.h
armarx::navigation::core::Position
Eigen::Vector3f Position
Definition: basic_types.h:36
armarx::eLinearInterpolation
@ eLinearInterpolation
Definition: InterpolationType.h:34
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::exceptions::local::ControlPointsInterpolationMatchException
Definition: ControlPointsInterpolationMatchException.h:42
armarx::InterpolationSegmentFactory::getSmallestDominating
static VirtualRobot::IKSolver::CartesianSelection getSmallestDominating(std::vector< VirtualRobot::IKSolver::CartesianSelection > selections)
getSmallestDominating returns the cartesian selection that dominates all other cartesian selections i...
Definition: InterpolationSegmentFactory.cpp:304
armarx::InterpolationSegmentFactory::produceSplineInterpolationSegments
static std::vector< AbstractInterpolationPtr > produceSplineInterpolationSegments(std::vector< PoseBasePtr > controlPoints)
produceInterpolationSegments constructs a vector of SplineInterpolations
Definition: InterpolationSegmentFactory.cpp:165
armarx::SplineInterpolation
The SplineInterpolation class represents a linear Interpolation between a series of control points Sp...
Definition: SplineInterpolation.h:36