moveit2
The MoveIt Motion Planning Framework for ROS 2.
planar_joint_model.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Ioan A. Sucan
5  * Copyright (c) 2008-2013, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the Willow Garage nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Ioan Sucan */
37 
38 #include <angles/angles.h>
39 #include <cmath>
40 #include <geometric_shapes/check_isometry.h>
41 #include <limits>
43 
44 namespace moveit
45 {
46 namespace core
47 {
48 PlanarJointModel::PlanarJointModel(const std::string& name, size_t joint_index, size_t first_variable_index)
49  : JointModel(name, joint_index, first_variable_index)
50  , angular_distance_weight_(1.0)
51  , motion_model_(HOLONOMIC)
52  , min_translational_distance_(1e-5)
53 {
54  type_ = PLANAR;
55 
56  local_variable_names_.push_back("x");
57  local_variable_names_.push_back("y");
58  local_variable_names_.push_back("theta");
59  for (int i = 0; i < 3; ++i)
60  {
61  variable_names_.push_back(getName() + "/" + local_variable_names_[i]);
63  }
64 
65  variable_bounds_.resize(3);
66  variable_bounds_[0].position_bounded_ = true;
67  variable_bounds_[1].position_bounded_ = true;
68  variable_bounds_[2].position_bounded_ = false;
69 
70  variable_bounds_[0].min_position_ = -std::numeric_limits<double>::infinity();
71  variable_bounds_[0].max_position_ = std::numeric_limits<double>::infinity();
72  variable_bounds_[1].min_position_ = -std::numeric_limits<double>::infinity();
73  variable_bounds_[1].max_position_ = std::numeric_limits<double>::infinity();
74  variable_bounds_[2].min_position_ = -M_PI;
75  variable_bounds_[2].max_position_ = M_PI;
76 
78 }
79 
81 {
82  return 3;
83 }
84 
85 double PlanarJointModel::getMaximumExtent(const Bounds& other_bounds) const
86 {
87  double dx = other_bounds[0].max_position_ - other_bounds[0].min_position_;
88  double dy = other_bounds[1].max_position_ - other_bounds[1].min_position_;
89  return sqrt(dx * dx + dy * dy) + M_PI * angular_distance_weight_;
90 }
91 
92 void PlanarJointModel::getVariableDefaultPositions(double* values, const Bounds& bounds) const
93 {
94  for (unsigned int i = 0; i < 2; ++i)
95  {
96  // if zero is a valid value
97  if (bounds[i].min_position_ <= 0.0 && bounds[i].max_position_ >= 0.0)
98  values[i] = 0.0;
99  else
100  values[i] = (bounds[i].min_position_ + bounds[i].max_position_) / 2.0;
101  }
102  values[2] = 0.0;
103 }
104 
105 void PlanarJointModel::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values,
106  const Bounds& bounds) const
107 {
108  if (bounds[0].max_position_ >= std::numeric_limits<double>::infinity() ||
109  bounds[0].min_position_ <= -std::numeric_limits<double>::infinity())
110  values[0] = 0.0;
111  else
112  values[0] = rng.uniformReal(bounds[0].min_position_, bounds[0].max_position_);
113  if (bounds[1].max_position_ >= std::numeric_limits<double>::infinity() ||
114  bounds[1].min_position_ <= -std::numeric_limits<double>::infinity())
115  values[1] = 0.0;
116  else
117  values[1] = rng.uniformReal(bounds[1].min_position_, bounds[1].max_position_);
118  values[2] = rng.uniformReal(bounds[2].min_position_, bounds[2].max_position_);
119 }
120 
121 void PlanarJointModel::getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values,
122  const Bounds& bounds, const double* near,
123  const double distance) const
124 {
125  if (bounds[0].max_position_ >= std::numeric_limits<double>::infinity() ||
126  bounds[0].min_position_ <= -std::numeric_limits<double>::infinity())
127  values[0] = 0.0;
128  else
129  values[0] = rng.uniformReal(std::max(bounds[0].min_position_, near[0] - distance),
130  std::min(bounds[0].max_position_, near[0] + distance));
131  if (bounds[1].max_position_ >= std::numeric_limits<double>::infinity() ||
132  bounds[1].min_position_ <= -std::numeric_limits<double>::infinity())
133  values[1] = 0.0;
134  else
135  values[1] = rng.uniformReal(std::max(bounds[1].min_position_, near[1] - distance),
136  std::min(bounds[1].max_position_, near[1] + distance));
137 
138  double da = angular_distance_weight_ * distance;
139  // limit the sampling range to 2pi to work correctly even if the distance is very large
140  if (da > M_PI)
141  da = M_PI;
142  values[2] = rng.uniformReal(near[2] - da, near[2] + da);
143  normalizeRotation(values);
144 }
145 
146 void computeTurnDriveTurnGeometry(const double* from, const double* to, const double min_translational_distance,
147  double& dx, double& dy, double& initial_turn, double& drive_angle, double& final_turn)
148 {
149  dx = to[0] - from[0];
150  dy = to[1] - from[1];
151  // If the translational distance between from & to states is very small, it will cause an unnecessary rotation since
152  // the robot will try to do the following rather than rotating directly to the orientation of `to` state
153  // 1- Align itself with the line connecting the origin of both states
154  // 2- Move to the origin of `to` state
155  // 3- Rotate so it have the same orientation as `to` state
156  // Example: from=[0.0, 0.0, 0.0] - to=[1e-31, 1e-31, -130°]
157  // here the robot will: rotate 45° -> move to the origin of `to` state -> rotate -175°, rather than rotating directly
158  // to -130°
159  // to fix this we added a joint property (default value is 1e-5) and make the movement pure rotation if the
160  // translational distance is less than this number
161  const double angle_straight_diff = std::hypot(dx, dy) > min_translational_distance ?
162  angles::shortest_angular_distance(from[2], std::atan2(dy, dx)) :
163  0.0;
164  const double angle_backward_diff = angles::normalize_angle(angle_straight_diff - M_PI);
165  const double move_straight_cost =
166  std::abs(angle_straight_diff) + std::abs(angles::shortest_angular_distance(from[2] + angle_straight_diff, to[2]));
167  const double move_backward_cost =
168  std::abs(angle_backward_diff) + std::abs(angles::shortest_angular_distance(from[2] + angle_backward_diff, to[2]));
169  if (move_straight_cost <= move_backward_cost)
170  {
171  initial_turn = angle_straight_diff;
172  }
173  else
174  {
175  initial_turn = angle_backward_diff;
176  }
177  drive_angle = from[2] + initial_turn;
178  final_turn = angles::shortest_angular_distance(drive_angle, to[2]);
179 }
180 
181 void PlanarJointModel::interpolate(const double* from, const double* to, const double t, double* state) const
182 {
183  if (motion_model_ == HOLONOMIC)
184  {
185  // interpolate position
186  state[0] = from[0] + (to[0] - from[0]) * t;
187  state[1] = from[1] + (to[1] - from[1]) * t;
188 
189  // interpolate angle
190  double diff = to[2] - from[2];
191  if (fabs(diff) <= M_PI)
192  state[2] = from[2] + diff * t;
193  else
194  {
195  if (diff > 0.0)
196  diff = 2.0 * M_PI - diff;
197  else
198  diff = -2.0 * M_PI - diff;
199  state[2] = from[2] - diff * t;
200  // input states are within bounds, so the following check is sufficient
201  if (state[2] > M_PI)
202  state[2] -= 2.0 * M_PI;
203  else if (state[2] < -M_PI)
204  state[2] += 2.0 * M_PI;
205  }
206  }
207  else if (motion_model_ == DIFF_DRIVE)
208  {
209  double dx, dy, initial_turn, drive_angle, final_turn;
210  computeTurnDriveTurnGeometry(from, to, min_translational_distance_, dx, dy, initial_turn, drive_angle, final_turn);
211 
212  double initial_d = fabs(initial_turn) * angular_distance_weight_;
213  double drive_d = hypot(dx, dy);
214  double final_d = fabs(final_turn) * angular_distance_weight_;
215 
216  double total_d = initial_d + drive_d + final_d;
217 
218  double initial_frac = initial_d / total_d;
219  double drive_frac = drive_d / total_d;
220  double final_frac = final_d / total_d;
221 
222  double percent;
223  if (t <= initial_frac)
224  {
225  percent = t / initial_frac;
226  state[0] = from[0];
227  state[1] = from[1];
228  state[2] = from[2] + initial_turn * percent;
229  }
230  else if (t <= initial_frac + drive_frac)
231  {
232  percent = (t - initial_frac) / drive_frac;
233  state[0] = from[0] + dx * percent;
234  state[1] = from[1] + dy * percent;
235  state[2] = drive_angle;
236  }
237  else
238  {
239  percent = (t - initial_frac - drive_frac) / final_frac;
240  state[0] = to[0];
241  state[1] = to[1];
242  state[2] = drive_angle + final_turn * percent;
243  }
244  }
245 }
246 
247 double PlanarJointModel::distance(const double* values1, const double* values2) const
248 {
249  if (motion_model_ == HOLONOMIC)
250  {
251  double dx = values1[0] - values2[0];
252  double dy = values1[1] - values2[1];
253 
254  double d = fabs(values1[2] - values2[2]);
255  d = (d > M_PI) ? 2.0 * M_PI - d : d;
256  return sqrt(dx * dx + dy * dy) + angular_distance_weight_ * d;
257  }
258  else if (motion_model_ == DIFF_DRIVE)
259  {
260  double dx, dy, initial_turn, drive_angle, final_turn;
261  computeTurnDriveTurnGeometry(values1, values2, min_translational_distance_, dx, dy, initial_turn, drive_angle,
262  final_turn);
263  return hypot(dx, dy) + angular_distance_weight_ * (fabs(initial_turn) + fabs(final_turn));
264  }
265 
266  return 0.0;
267 }
268 
269 bool PlanarJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds, double margin) const
270 {
271  for (unsigned int i = 0; i < 3; ++i)
272  if (values[i] < bounds[i].min_position_ - margin || values[i] > bounds[i].max_position_ + margin)
273  return false;
274  return true;
275 }
276 
277 bool PlanarJointModel::normalizeRotation(double* values) const
278 {
279  double& v = values[2];
280  if (v >= -M_PI && v <= M_PI)
281  return false;
282  v = fmod(v, 2.0 * M_PI);
283  if (v < -M_PI)
284  v += 2.0 * M_PI;
285  else if (v > M_PI)
286  v -= 2.0 * M_PI;
287  return true;
288 }
289 
290 bool PlanarJointModel::enforcePositionBounds(double* values, const Bounds& bounds) const
291 {
292  bool result = normalizeRotation(values);
293  for (unsigned int i = 0; i < 2; ++i)
294  {
295  if (values[i] < bounds[i].min_position_)
296  {
297  values[i] = bounds[i].min_position_;
298  result = true;
299  }
300  else if (values[i] > bounds[i].max_position_)
301  {
302  values[i] = bounds[i].max_position_;
303  result = true;
304  }
305  }
306  return result;
307 }
308 
309 void PlanarJointModel::computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const
310 {
311  transf = Eigen::Isometry3d(Eigen::Translation3d(joint_values[0], joint_values[1], 0.0) *
312  Eigen::AngleAxisd(joint_values[2], Eigen::Vector3d::UnitZ()));
313 }
314 
315 void PlanarJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const
316 {
317  joint_values[0] = transf.translation().x();
318  joint_values[1] = transf.translation().y();
319 
320  ASSERT_ISOMETRY(transf) // unsanitized input, could contain a non-isometry
321  Eigen::Quaterniond q(transf.linear());
322  // taken from Bullet
323  double s_squared = 1.0 - (q.w() * q.w());
324  if (s_squared < 10.0 * std::numeric_limits<double>::epsilon())
325  joint_values[2] = 0.0;
326  else
327  {
328  double s = 1.0 / sqrt(s_squared);
329  joint_values[2] = (acos(q.w()) * 2.0f) * (q.z() * s);
330  }
331 }
332 
333 } // end of namespace core
334 } // end of namespace moveit
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:117
VariableIndexMap variable_index_map_
Map from variable names to the corresponding index in variable_names_ (indexing makes sense within th...
Definition: joint_model.h:488
JointType type_
The type of joint.
Definition: joint_model.h:473
Bounds variable_bounds_
The bounds for each variable (low, high) in the same order as variable_names_.
Definition: joint_model.h:482
std::vector< VariableBounds > Bounds
The datatype for the joint bounds.
Definition: joint_model.h:131
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:145
std::vector< std::string > local_variable_names_
The local names to use for the variables that make up this joint.
Definition: joint_model.h:476
double getMaximumExtent() const
Definition: joint_model.h:441
std::vector< std::string > variable_names_
The full names to use for the variables that make up this joint.
Definition: joint_model.h:479
bool normalizeRotation(double *values) const
void interpolate(const double *from, const double *to, const double t, double *state) const override
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state....
void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, double *values, const Bounds &other_bounds, const double *near, const double distance) const override
Provide random values for the joint variables (within specified bounds). Enough memory is assumed to ...
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values, const Bounds &other_bounds) const override
Provide random values for the joint variables (within specified bounds). Enough memory is assumed to ...
PlanarJointModel(const std::string &name, size_t joint_index, size_t first_variable_index)
void computeTransform(const double *joint_values, Eigen::Isometry3d &transf) const override
Given the joint values for a joint, compute the corresponding transform. The computed transform is gu...
bool satisfiesPositionBounds(const double *values, const Bounds &other_bounds, double margin) const override
Check if the set of position values for the variables of this joint are within bounds,...
void computeVariablePositions(const Eigen::Isometry3d &transf, double *joint_values) const override
Given the transform generated by joint, compute the corresponding joint values. Make sure the passed ...
unsigned int getStateSpaceDimension() const override
Get the dimension of the state space that corresponds to this joint.
bool enforcePositionBounds(double *values, const Bounds &other_bounds) const override
Force the specified values to be inside bounds and normalized. Quaternions are normalized,...
void getVariableDefaultPositions(double *values, const Bounds &other_bounds) const override
Provide a default value for the joint given the joint variable bounds. Most joints will use the defau...
double distance(const double *values1, const double *values2) const override
Compute the distance between two joint states of the same model (represented by the variable values)
void computeTurnDriveTurnGeometry(const double *from, const double *to, const double min_translational_distance, double &dx, double &dy, double &initial_turn, double &drive_angle, double &final_turn)
Compute the geometry to turn toward the target point, drive straight and then turn to target orientat...
Main namespace for MoveIt.
Definition: exceptions.h:43
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:55
d
Definition: setup.py:4
name
Definition: setup.py:7