floating_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 
40 #include <boost/math/constants/constants.hpp>
41 #include <ros/console.h>
42 #include <limits>
43 #include <cmath>
44 
45 namespace moveit
46 {
47 namespace core
48 {
49 namespace
50 {
51 constexpr size_t STATE_SPACE_DIMENSION = 7;
52 
53 } // namespace
54 
55 FloatingJointModel::FloatingJointModel(const std::string& name) : JointModel(name), angular_distance_weight_(1.0)
56 {
57  type_ = FLOATING;
58  local_variable_names_.push_back("trans_x");
59  local_variable_names_.push_back("trans_y");
60  local_variable_names_.push_back("trans_z");
61  local_variable_names_.push_back("rot_x");
62  local_variable_names_.push_back("rot_y");
63  local_variable_names_.push_back("rot_z");
64  local_variable_names_.push_back("rot_w");
65  for (size_t i = 0; i < STATE_SPACE_DIMENSION; ++i)
66  {
67  variable_names_.push_back(name_ + "/" + local_variable_names_[i]);
68  variable_index_map_[variable_names_.back()] = i;
69  }
70 
71  variable_bounds_.resize(7);
72 
73  variable_bounds_[0].position_bounded_ = true;
74  variable_bounds_[1].position_bounded_ = true;
75  variable_bounds_[2].position_bounded_ = true;
76  variable_bounds_[3].position_bounded_ = true;
77  variable_bounds_[4].position_bounded_ = true;
78  variable_bounds_[5].position_bounded_ = true;
79  variable_bounds_[6].position_bounded_ = true;
80 
81  variable_bounds_[0].min_position_ = -std::numeric_limits<double>::infinity();
82  variable_bounds_[0].max_position_ = std::numeric_limits<double>::infinity();
83  variable_bounds_[1].min_position_ = -std::numeric_limits<double>::infinity();
84  variable_bounds_[1].max_position_ = std::numeric_limits<double>::infinity();
85  variable_bounds_[2].min_position_ = -std::numeric_limits<double>::infinity();
86  variable_bounds_[2].max_position_ = std::numeric_limits<double>::infinity();
87  variable_bounds_[3].min_position_ = -1.0;
88  variable_bounds_[3].max_position_ = 1.0;
89  variable_bounds_[4].min_position_ = -1.0;
90  variable_bounds_[4].max_position_ = 1.0;
91  variable_bounds_[5].min_position_ = -1.0;
92  variable_bounds_[5].max_position_ = 1.0;
93  variable_bounds_[6].min_position_ = -1.0;
94  variable_bounds_[6].max_position_ = 1.0;
95 
96  computeVariableBoundsMsg();
97 }
98 
99 double FloatingJointModel::getMaximumExtent(const Bounds& other_bounds) const
100 {
101  double dx = other_bounds[0].max_position_ - other_bounds[0].min_position_;
102  double dy = other_bounds[1].max_position_ - other_bounds[1].min_position_;
103  double dz = other_bounds[2].max_position_ - other_bounds[2].min_position_;
104  return sqrt(dx * dx + dy * dy + dz * dz) + boost::math::constants::pi<double>() * 0.5 * angular_distance_weight_;
105 }
106 
107 double FloatingJointModel::distance(const double* values1, const double* values2) const
108 {
109  return distanceTranslation(values1, values2) + angular_distance_weight_ * distanceRotation(values1, values2);
110 }
111 
112 double FloatingJointModel::distanceTranslation(const double* values1, const double* values2) const
113 {
114  double dx = values1[0] - values2[0];
115  double dy = values1[1] - values2[1];
116  double dz = values1[2] - values2[2];
117  return sqrt(dx * dx + dy * dy + dz * dz);
118 }
119 
120 double FloatingJointModel::distanceRotation(const double* values1, const double* values2) const
121 {
122  double dq =
123  fabs(values1[3] * values2[3] + values1[4] * values2[4] + values1[5] * values2[5] + values1[6] * values2[6]);
124  if (dq + std::numeric_limits<double>::epsilon() >= 1.0)
125  return 0.0;
126  else
127  return acos(dq);
128 }
129 
130 void FloatingJointModel::interpolate(const double* from, const double* to, const double t, double* state) const
131 {
132  // interpolate position
133  state[0] = from[0] + (to[0] - from[0]) * t;
134  state[1] = from[1] + (to[1] - from[1]) * t;
135  state[2] = from[2] + (to[2] - from[2]) * t;
136 
137  double dq = fabs(from[3] * to[3] + from[4] * to[4] + from[5] * to[5] + from[6] * to[6]);
138  double theta = (dq + std::numeric_limits<double>::epsilon() >= 1.0) ? 0.0 : acos(dq);
139  if (theta > std::numeric_limits<double>::epsilon())
140  {
141  double d = 1.0 / sin(theta);
142  double s0 = sin((1.0 - t) * theta);
143  double s1 = sin(t * theta);
144  if (dq < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
145  s1 = -s1;
146  state[3] = (from[3] * s0 + to[3] * s1) * d;
147  state[4] = (from[4] * s0 + to[4] * s1) * d;
148  state[5] = (from[5] * s0 + to[5] * s1) * d;
149  state[6] = (from[6] * s0 + to[6] * s1) * d;
150  }
151  else
152  {
153  state[3] = from[3];
154  state[4] = from[4];
155  state[5] = from[5];
156  state[6] = from[6];
157  }
158 }
159 
160 bool FloatingJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds, double margin) const
161 {
162  if (values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin)
163  return false;
164  if (values[1] < bounds[1].min_position_ - margin || values[1] > bounds[1].max_position_ + margin)
165  return false;
166  if (values[2] < bounds[2].min_position_ - margin || values[2] > bounds[2].max_position_ + margin)
167  return false;
168  double norm_sqr = values[3] * values[3] + values[4] * values[4] + values[5] * values[5] + values[6] * values[6];
169  return fabs(norm_sqr - 1.0) <= std::numeric_limits<float>::epsilon() * 10.0;
170 }
171 
172 bool FloatingJointModel::normalizeRotation(double* values) const
173 {
174  // normalize the quaternion if we need to
175  double norm_sqr = values[3] * values[3] + values[4] * values[4] + values[5] * values[5] + values[6] * values[6];
176  if (fabs(norm_sqr - 1.0) > std::numeric_limits<double>::epsilon() * 100.0)
177  {
178  double norm = sqrt(norm_sqr);
179  if (norm < std::numeric_limits<double>::epsilon() * 100.0)
180  {
181  ROS_WARN_NAMED("robot_model", "Quaternion is zero in RobotState representation. Setting to identity");
182  values[3] = 0.0;
183  values[4] = 0.0;
184  values[5] = 0.0;
185  values[6] = 1.0;
186  }
187  else
188  {
189  values[3] /= norm;
190  values[4] /= norm;
191  values[5] /= norm;
192  values[6] /= norm;
193  }
194  return true;
195  }
196  else
197  return false;
198 }
199 
200 unsigned int FloatingJointModel::getStateSpaceDimension() const
201 {
202  return STATE_SPACE_DIMENSION;
203 }
204 
205 bool FloatingJointModel::enforcePositionBounds(double* values, const Bounds& bounds) const
206 {
207  bool result = normalizeRotation(values);
208  for (unsigned int i = 0; i < 3; ++i)
209  {
210  if (values[i] < bounds[i].min_position_)
211  {
212  values[i] = bounds[i].min_position_;
213  result = true;
214  }
215  else if (values[i] > bounds[i].max_position_)
216  {
217  values[i] = bounds[i].max_position_;
218  result = true;
219  }
220  }
221  return result;
222 }
223 
224 void FloatingJointModel::computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const
225 {
226  transf = Eigen::Isometry3d(
227  Eigen::Translation3d(joint_values[0], joint_values[1], joint_values[2]) *
228  Eigen::Quaterniond(joint_values[6], joint_values[3], joint_values[4], joint_values[5]).normalized());
229 }
230 
231 void FloatingJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const
232 {
233  joint_values[0] = transf.translation().x();
234  joint_values[1] = transf.translation().y();
235  joint_values[2] = transf.translation().z();
236  ASSERT_ISOMETRY(transf) // unsanitized input, could contain non-isometry
237  Eigen::Quaterniond q(transf.linear());
238  joint_values[3] = q.x();
239  joint_values[4] = q.y();
240  joint_values[5] = q.z();
241  joint_values[6] = q.w();
242 }
243 
244 void FloatingJointModel::getVariableDefaultPositions(double* values, const Bounds& bounds) const
245 {
246  for (unsigned int i = 0; i < 3; ++i)
247  {
248  // if zero is a valid value
249  if (bounds[i].min_position_ <= 0.0 && bounds[i].max_position_ >= 0.0)
250  values[i] = 0.0;
251  else
252  values[i] = (bounds[i].min_position_ + bounds[i].max_position_) / 2.0;
253  }
254 
255  values[3] = 0.0;
256  values[4] = 0.0;
257  values[5] = 0.0;
258  values[6] = 1.0;
259 }
260 
261 void FloatingJointModel::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values,
262  const Bounds& bounds) const
263 {
264  if (bounds[0].max_position_ >= std::numeric_limits<double>::infinity() ||
265  bounds[0].min_position_ <= -std::numeric_limits<double>::infinity())
266  values[0] = 0.0;
267  else
268  values[0] = rng.uniformReal(bounds[0].min_position_, bounds[0].max_position_);
269  if (bounds[1].max_position_ >= std::numeric_limits<double>::infinity() ||
270  bounds[1].min_position_ <= -std::numeric_limits<double>::infinity())
271  values[1] = 0.0;
272  else
273  values[1] = rng.uniformReal(bounds[1].min_position_, bounds[1].max_position_);
274  if (bounds[2].max_position_ >= std::numeric_limits<double>::infinity() ||
275  bounds[2].min_position_ <= -std::numeric_limits<double>::infinity())
276  values[2] = 0.0;
277  else
278  values[2] = rng.uniformReal(bounds[2].min_position_, bounds[2].max_position_);
279 
280  double q[4];
281  rng.quaternion(q);
282  values[3] = q[0];
283  values[4] = q[1];
284  values[5] = q[2];
285  values[6] = q[3];
286 }
287 
288 void FloatingJointModel::getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values,
289  const Bounds& bounds, const double* near,
290  const double distance) const
291 {
292  if (bounds[0].max_position_ >= std::numeric_limits<double>::infinity() ||
293  bounds[0].min_position_ <= -std::numeric_limits<double>::infinity())
294  values[0] = 0.0;
295  else
296  values[0] = rng.uniformReal(std::max(bounds[0].min_position_, near[0] - distance),
297  std::min(bounds[0].max_position_, near[0] + distance));
298  if (bounds[1].max_position_ >= std::numeric_limits<double>::infinity() ||
299  bounds[1].min_position_ <= -std::numeric_limits<double>::infinity())
300  values[1] = 0.0;
301  else
302  values[1] = rng.uniformReal(std::max(bounds[1].min_position_, near[1] - distance),
303  std::min(bounds[1].max_position_, near[1] + distance));
304  if (bounds[2].max_position_ >= std::numeric_limits<double>::infinity() ||
305  bounds[2].min_position_ <= -std::numeric_limits<double>::infinity())
306  values[2] = 0.0;
307  else
308  values[2] = rng.uniformReal(std::max(bounds[2].min_position_, near[2] - distance),
309  std::min(bounds[2].max_position_, near[2] + distance));
310 
311  double da = angular_distance_weight_ * distance;
312  if (da >= .25 * boost::math::constants::pi<double>())
313  {
314  double q[4];
315  rng.quaternion(q);
316  values[3] = q[0];
317  values[4] = q[1];
318  values[5] = q[2];
319  values[6] = q[3];
320  }
321  else
322  {
323  // taken from OMPL
324  // sample angle & axis
325  double ax = rng.gaussian01();
326  double ay = rng.gaussian01();
327  double az = rng.gaussian01();
328  double angle = 2.0 * pow(rng.uniform01(), 1.0 / 3.0) * da;
329  // convert to quaternion
330  double q[4];
331  double norm = sqrt(ax * ax + ay * ay + az * az);
332  if (norm < 1e-6)
333  {
334  q[0] = q[1] = q[2] = 0.0;
335  q[3] = 1.0;
336  }
337  else
338  {
339  double s = sin(angle / 2.0);
340  q[0] = s * ax / norm;
341  q[1] = s * ay / norm;
342  q[2] = s * az / norm;
343  q[3] = cos(angle / 2.0);
344  }
345  // multiply quaternions: near * q
346  values[3] = near[6] * q[0] + near[3] * q[3] + near[4] * q[2] - near[5] * q[1];
347  values[4] = near[6] * q[1] + near[4] * q[3] + near[5] * q[0] - near[3] * q[2];
348  values[5] = near[6] * q[2] + near[5] * q[3] + near[3] * q[1] - near[4] * q[0];
349  values[6] = near[6] * q[3] - near[3] * q[0] - near[4] * q[1] - near[5] * q[2];
350  }
351 }
352 
353 } // end of namespace core
354 } // end of namespace moveit
random_numbers::RandomNumberGenerator::uniform01
double uniform01(void)
random_numbers::RandomNumberGenerator::gaussian01
double gaussian01(void)
s
XmlRpcServer s
check_isometry.h
random_numbers::RandomNumberGenerator::uniformReal
double uniformReal(double lower_bound, double upper_bound)
console.h
name
std::string name
random_numbers::RandomNumberGenerator::quaternion
void quaternion(double value[4])
moveit::core::FloatingJointModel::FloatingJointModel
FloatingJointModel(const std::string &name)
Definition: floating_joint_model.cpp:121
ASSERT_ISOMETRY
#define ASSERT_ISOMETRY(transform)
d
d
random_numbers::RandomNumberGenerator
moveit
Main namespace for MoveIt.
Definition: background_processing.h:46
values
std::vector< double > values
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
angle
TF2SIMD_FORCE_INLINE tf2Scalar angle(const Quaternion &q1, const Quaternion &q2)
floating_joint_model.h
pr2_arm_kinematics::distance
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:86


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Dec 12 2020 03:25:44