CollisionCheckUtil.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package RobotComponents
19  * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
20  * @date 2015
21  * @copyright http://www.gnu.org/licenses/gpl.txt
22  * GNU General Public License
23  */
24 #pragma once
25 
26 #include <Eigen/Core>
27 
29 
30 #include <optional>
31 
32 namespace armarx
33 {
34  /**
35  * @brief Tries to reach to from from using the given stepsize.
36  * @param from The starting point.
37  * @param to The target point.
38  * @param dcdStepSize The used stepsize.
39  * @param isCollisionFree The collision checker used to test configuration for collision.
40  * @return Returns the point p that maximizes distance(from,p) while the line(from, p)
41  * is collision free at all points with a distance of k * dcdStepSize from from.
42  * If no such point is reachable exactly from is returned.
43  * If to is reachable exactly to is returned.
44  */
45  template<class RealType, class CollisionChecker, class ConfigType>
46  ConfigType dcdSteer(const ConfigType& from, const ConfigType& to, RealType dcdStepSize, CollisionChecker isCollisionFree)
47  {
49  ARMARX_CHECK_EXPRESSION(from.size() == to.size());
50  const auto size = static_cast<long int>(from.size()); //eigen wants long int
51  //get eigen views
52  Eigen::Map<const EigenVector> eFrom {from.data(), size};
53  Eigen::Map<const EigenVector> eTo {to.data(), size};
54 
55  EigenVector step = eTo - eFrom;
56  const auto length = step.norm();
57  step *= dcdStepSize / length; // if length == 0 this values does not get used
58 
59  //will truncate in most cases => we will need to check the destination step for collision
60  ARMARX_CHECK_EXPRESSION(length / dcdStepSize < std::numeric_limits<RealType>::infinity());
61  ARMARX_CHECK_EXPRESSION(length / dcdStepSize < static_cast<RealType>(std::numeric_limits<std::size_t>::max()));
62  const std::size_t stepsToTake = static_cast<std::size_t>(length / dcdStepSize);
63 
64  ConfigType stepToCheck(from);
65  ARMARX_CHECK_EXPRESSION(stepToCheck.size() == from.size());
66  Eigen::Map<EigenVector> eStepToCheck {stepToCheck.data(), size};
67  std::size_t stepsTaken = 0;
68 
69  for (; stepsTaken < stepsToTake; ++stepsTaken)
70  {
71  eStepToCheck += step;
72 
73  if (!isCollisionFree(stepToCheck))
74  {
75  if (0 == stepsTaken)
76  {
77  //could not move!
78  //return an exact copy
79  return from;
80  }
81 
82  eStepToCheck -= step;
83  return stepToCheck;
84  }
85  }
86 
87  ARMARX_CHECK_EXPRESSION(stepsTaken == stepsToTake);
88 
89  //check to
90  if (isCollisionFree(to))
91  {
92  //return an exact copy
93  return to;
94  }
95 
96  //to has collision or was not reached
97  return stepToCheck;
98  }
99 
100 
101 
102 
103  //startCfg is assumed to be collision free
104  /**
105  * @brief Returns whether the line startCfg to to is collision free.
106  * @param from The config to startCfg from. (startCfg is assumed to be collision free.
107  * @param to The config to reach
108  * @param dcdStepSize The dcd step size
109  * @param isCollisionFree The collision checker returning true, if a config is collision free.
110  * @param toIsCollisionFree Whether to is collision free
111  * @return Whether the line startCfg to to is collision free.
112  */
113  template<class RealType, class CollisionChecker, class ConfigType, class Distance = std::function<float(Eigen::VectorXf, Eigen::VectorXf)>,
114  class Interpolate = std::function<Eigen::VectorXf(Eigen::VectorXf, Eigen::VectorXf, float)>>
115  bool dcdIsPathCollisionFree(const ConfigType& from, const ConfigType& to, RealType dcdStepSize, CollisionChecker isCollisionFree, bool toIsCollisionFree = true,
116  const std::optional<Distance>& distanceLambda = std::optional<Distance>(),
117  const std::optional<Interpolate>& interpolationLambda = std::optional<Interpolate>(), std::vector<ConfigType>* resultPath = NULL)
118  {
119  if (!(toIsCollisionFree || isCollisionFree(to)))
120  {
121  return false;
122  }
123 
125  ARMARX_CHECK_EXPRESSION(from.size() == to.size());
126  const auto size = static_cast<long int>(from.size()); //eigen wants long int
127  //get eigen views
128  Eigen::Map<const EigenVector> eFrom {from.data(), size};
129  Eigen::Map<const EigenVector> eTo {to.data(), size};
130  EigenVector connecting = eTo - eFrom;
131  std::vector<float> tmp(size);
132  Eigen::Map<EigenVector> eTmp {tmp.data(), size};
133  float distanceFromTo;
134  if (distanceLambda)
135  {
136  distanceFromTo = (*distanceLambda)(eFrom, eTo);
137  }
138  else
139  {
140  distanceFromTo = connecting.norm();
141  }
142  ARMARX_INFO << deactivateSpam(1) << VAROUT(dcdStepSize) << VAROUT(distanceFromTo) << VAROUT(eFrom) << VAROUT(eTo);
143  if (distanceFromTo <= dcdStepSize)
144  {
145  if (resultPath)
146  {
147  resultPath->push_back(to);
148  }
149  //since to is collision free
150  return true;
151  }
152 
153  float dcdAsDistanceFactor = dcdStepSize / distanceFromTo;
154 
155  float checkedStepFactor;
156  float betweenStepFactor;
157  std::size_t stepsToCheck = 1;
158 
159  do
160  {
161  betweenStepFactor = 1.f / stepsToCheck;
162  checkedStepFactor = betweenStepFactor / 2.f;
163 
164  float factor = checkedStepFactor;
165 
166  for (std::size_t step = 0; step < stepsToCheck; ++step, factor += betweenStepFactor)
167  {
168  if (interpolationLambda)
169  {
170  eTmp = (*interpolationLambda)(eFrom, eTo, factor);
171  }
172  else
173  {
174  eTmp = eFrom + factor * connecting;
175  }
176  if (!isCollisionFree(tmp))
177  {
178  return false;
179  }
180  }
181  if (resultPath)
182  {
183  resultPath->push_back(tmp);
184  }
185  //increment steps
186  stepsToCheck = stepsToCheck << 1;
187  }
188  while (checkedStepFactor > dcdAsDistanceFactor);
189 
190  //checked everything and no collision
191  return true;
192  }
193 }
armarx::dcdIsPathCollisionFree
bool dcdIsPathCollisionFree(const ConfigType &from, const ConfigType &to, RealType dcdStepSize, CollisionChecker isCollisionFree, bool toIsCollisionFree=true, const std::optional< Distance > &distanceLambda=std::optional< Distance >(), const std::optional< Interpolate > &interpolationLambda=std::optional< Interpolate >(), std::vector< ConfigType > *resultPath=NULL)
Returns whether the line startCfg to to is collision free.
Definition: CollisionCheckUtil.h:115
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:72
armarx::dcdSteer
ConfigType dcdSteer(const ConfigType &from, const ConfigType &to, RealType dcdStepSize, CollisionChecker isCollisionFree)
Tries to reach to from from using the given stepsize.
Definition: CollisionCheckUtil.h:46
max
T max(T t1, T t2)
Definition: gdiam.h:48
ExpressionException.h
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
Eigen::Matrix
Definition: EigenForwardDeclarations.h:27
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28