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 
39 #include <boost/math/constants/constants.hpp>
40 #include <console_bridge/console.h>
41 #include <limits>
42 #include <cmath>
43 
45  : JointModel(name), angular_distance_weight_(1.0)
46 {
47  type_ = FLOATING;
48  local_variable_names_.push_back("trans_x");
49  local_variable_names_.push_back("trans_y");
50  local_variable_names_.push_back("trans_z");
51  local_variable_names_.push_back("rot_x");
52  local_variable_names_.push_back("rot_y");
53  local_variable_names_.push_back("rot_z");
54  local_variable_names_.push_back("rot_w");
55  for (int i = 0; i < 7; ++i)
56  {
57  variable_names_.push_back(name_ + "/" + local_variable_names_[i]);
59  }
60 
61  variable_bounds_.resize(7);
62 
63  variable_bounds_[0].position_bounded_ = true;
64  variable_bounds_[1].position_bounded_ = true;
65  variable_bounds_[2].position_bounded_ = true;
66  variable_bounds_[3].position_bounded_ = true;
67  variable_bounds_[4].position_bounded_ = true;
68  variable_bounds_[5].position_bounded_ = true;
69  variable_bounds_[6].position_bounded_ = true;
70 
71  variable_bounds_[0].min_position_ = -std::numeric_limits<double>::infinity();
72  variable_bounds_[0].max_position_ = std::numeric_limits<double>::infinity();
73  variable_bounds_[1].min_position_ = -std::numeric_limits<double>::infinity();
74  variable_bounds_[1].max_position_ = std::numeric_limits<double>::infinity();
75  variable_bounds_[2].min_position_ = -std::numeric_limits<double>::infinity();
76  variable_bounds_[2].max_position_ = std::numeric_limits<double>::infinity();
77  variable_bounds_[3].min_position_ = -1.0;
78  variable_bounds_[3].max_position_ = 1.0;
79  variable_bounds_[4].min_position_ = -1.0;
80  variable_bounds_[4].max_position_ = 1.0;
81  variable_bounds_[5].min_position_ = -1.0;
82  variable_bounds_[5].max_position_ = 1.0;
83  variable_bounds_[6].min_position_ = -1.0;
84  variable_bounds_[6].max_position_ = 1.0;
85 
87 }
88 
90 {
91  double dx = other_bounds[0].max_position_ - other_bounds[0].min_position_;
92  double dy = other_bounds[1].max_position_ - other_bounds[1].min_position_;
93  double dz = other_bounds[2].max_position_ - other_bounds[2].min_position_;
94  return sqrt(dx * dx + dy * dy + dz * dz) + boost::math::constants::pi<double>() * 0.5 * angular_distance_weight_;
95 }
96 
97 double moveit::core::FloatingJointModel::distance(const double* values1, const double* values2) const
98 {
99  return distanceTranslation(values1, values2) + angular_distance_weight_ * distanceRotation(values1, values2);
100 }
101 
102 double moveit::core::FloatingJointModel::distanceTranslation(const double* values1, const double* values2) const
103 {
104  double dx = values1[0] - values2[0];
105  double dy = values1[1] - values2[1];
106  double dz = values1[2] - values2[2];
107  return sqrt(dx * dx + dy * dy + dz * dz);
108 }
109 
110 double moveit::core::FloatingJointModel::distanceRotation(const double* values1, const double* values2) const
111 {
112  double dq =
113  fabs(values1[3] * values2[3] + values1[4] * values2[4] + values1[5] * values2[5] + values1[6] * values2[6]);
114  if (dq + std::numeric_limits<double>::epsilon() >= 1.0)
115  return 0.0;
116  else
117  return acos(dq);
118 }
119 
120 void moveit::core::FloatingJointModel::interpolate(const double* from, const double* to, const double t,
121  double* state) const
122 {
123  // interpolate position
124  state[0] = from[0] + (to[0] - from[0]) * t;
125  state[1] = from[1] + (to[1] - from[1]) * t;
126  state[2] = from[2] + (to[2] - from[2]) * t;
127 
128  double dq = fabs(from[3] * to[3] + from[4] * to[4] + from[5] * to[5] + from[6] * to[6]);
129  double theta = (dq + std::numeric_limits<double>::epsilon() >= 1.0) ? 0.0 : acos(dq);
130  if (theta > std::numeric_limits<double>::epsilon())
131  {
132  double d = 1.0 / sin(theta);
133  double s0 = sin((1.0 - t) * theta);
134  double s1 = sin(t * theta);
135  if (dq < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
136  s1 = -s1;
137  state[3] = (from[3] * s0 + to[3] * s1) * d;
138  state[4] = (from[4] * s0 + to[4] * s1) * d;
139  state[5] = (from[5] * s0 + to[5] * s1) * d;
140  state[6] = (from[6] * s0 + to[6] * s1) * d;
141  }
142  else
143  {
144  state[3] = from[3];
145  state[4] = from[4];
146  state[5] = from[5];
147  state[6] = from[6];
148  }
149 }
150 
151 bool moveit::core::FloatingJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds,
152  double margin) const
153 {
154  if (values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin)
155  return false;
156  if (values[1] < bounds[1].min_position_ - margin || values[1] > bounds[1].max_position_ + margin)
157  return false;
158  if (values[2] < bounds[2].min_position_ - margin || values[2] > bounds[2].max_position_ + margin)
159  return false;
160  double normSqr = values[3] * values[3] + values[4] * values[4] + values[5] * values[5] + values[6] * values[6];
161  if (fabs(normSqr - 1.0) > std::numeric_limits<float>::epsilon() * 10.0)
162  return false;
163  return true;
164 }
165 
167 {
168  // normalize the quaternion if we need to
169  double normSqr = values[3] * values[3] + values[4] * values[4] + values[5] * values[5] + values[6] * values[6];
170  if (fabs(normSqr - 1.0) > std::numeric_limits<double>::epsilon() * 100.0)
171  {
172  double norm = sqrt(normSqr);
173  if (norm < std::numeric_limits<double>::epsilon() * 100.0)
174  {
175  logWarn("Quaternion is zero in RobotState representation. Setting to identity");
176  values[3] = 0.0;
177  values[4] = 0.0;
178  values[5] = 0.0;
179  values[6] = 1.0;
180  }
181  else
182  {
183  values[3] /= norm;
184  values[4] /= norm;
185  values[5] /= norm;
186  values[6] /= norm;
187  }
188  return true;
189  }
190  else
191  return false;
192 }
193 
195 {
196  return 6;
197 }
198 
199 bool moveit::core::FloatingJointModel::enforcePositionBounds(double* values, const Bounds& bounds) const
200 {
201  bool result = normalizeRotation(values);
202  for (unsigned int i = 0; i < 3; ++i)
203  {
204  if (values[i] < bounds[i].min_position_)
205  {
206  values[i] = bounds[i].min_position_;
207  result = true;
208  }
209  else if (values[i] > bounds[i].max_position_)
210  {
211  values[i] = bounds[i].max_position_;
212  result = true;
213  }
214  }
215  return result;
216 }
217 
218 void moveit::core::FloatingJointModel::computeTransform(const double* joint_values, Eigen::Affine3d& transf) const
219 {
220  transf = Eigen::Affine3d(
221  Eigen::Translation3d(joint_values[0], joint_values[1], joint_values[2]) *
222  Eigen::Quaterniond(joint_values[6], joint_values[3], joint_values[4], joint_values[5]).toRotationMatrix());
223 }
224 
226  double* joint_values) const
227 {
228  joint_values[0] = transf.translation().x();
229  joint_values[1] = transf.translation().y();
230  joint_values[2] = transf.translation().z();
231  Eigen::Quaterniond q(transf.rotation());
232  joint_values[3] = q.x();
233  joint_values[4] = q.y();
234  joint_values[5] = q.z();
235  joint_values[6] = q.w();
236 }
237 
239 {
240  for (unsigned int i = 0; i < 3; ++i)
241  {
242  // if zero is a valid value
243  if (bounds[i].min_position_ <= 0.0 && bounds[i].max_position_ >= 0.0)
244  values[i] = 0.0;
245  else
246  values[i] = (bounds[i].min_position_ + bounds[i].max_position_) / 2.0;
247  }
248 
249  values[3] = 0.0;
250  values[4] = 0.0;
251  values[5] = 0.0;
252  values[6] = 1.0;
253 }
254 
256  double* values, const Bounds& bounds) const
257 {
258  if (bounds[0].max_position_ >= std::numeric_limits<double>::infinity() ||
259  bounds[0].min_position_ <= -std::numeric_limits<double>::infinity())
260  values[0] = 0.0;
261  else
262  values[0] = rng.uniformReal(bounds[0].min_position_, bounds[0].max_position_);
263  if (bounds[1].max_position_ >= std::numeric_limits<double>::infinity() ||
264  bounds[1].min_position_ <= -std::numeric_limits<double>::infinity())
265  values[1] = 0.0;
266  else
267  values[1] = rng.uniformReal(bounds[1].min_position_, bounds[1].max_position_);
268  if (bounds[2].max_position_ >= std::numeric_limits<double>::infinity() ||
269  bounds[2].min_position_ <= -std::numeric_limits<double>::infinity())
270  values[2] = 0.0;
271  else
272  values[2] = rng.uniformReal(bounds[2].min_position_, bounds[2].max_position_);
273 
274  double q[4];
275  rng.quaternion(q);
276  values[3] = q[0];
277  values[4] = q[1];
278  values[5] = q[2];
279  values[6] = q[3];
280 }
281 
283  double* values, const Bounds& bounds,
284  const double* near, const double distance) const
285 {
286  if (bounds[0].max_position_ >= std::numeric_limits<double>::infinity() ||
287  bounds[0].min_position_ <= -std::numeric_limits<double>::infinity())
288  values[0] = 0.0;
289  else
290  values[0] = rng.uniformReal(std::max(bounds[0].min_position_, near[0] - distance),
291  std::min(bounds[0].max_position_, near[0] + distance));
292  if (bounds[1].max_position_ >= std::numeric_limits<double>::infinity() ||
293  bounds[1].min_position_ <= -std::numeric_limits<double>::infinity())
294  values[1] = 0.0;
295  else
296  values[1] = rng.uniformReal(std::max(bounds[1].min_position_, near[1] - distance),
297  std::min(bounds[1].max_position_, near[1] + distance));
298  if (bounds[2].max_position_ >= std::numeric_limits<double>::infinity() ||
299  bounds[2].min_position_ <= -std::numeric_limits<double>::infinity())
300  values[2] = 0.0;
301  else
302  values[2] = rng.uniformReal(std::max(bounds[2].min_position_, near[2] - distance),
303  std::min(bounds[2].max_position_, near[2] + distance));
304 
305  double da = angular_distance_weight_ * distance;
306  if (da >= .25 * boost::math::constants::pi<double>())
307  {
308  double q[4];
309  rng.quaternion(q);
310  values[3] = q[0];
311  values[4] = q[1];
312  values[5] = q[2];
313  values[6] = q[3];
314  }
315  else
316  {
317  // taken from OMPL
318  // sample angle & axis
319  double ax = rng.gaussian01();
320  double ay = rng.gaussian01();
321  double az = rng.gaussian01();
322  double angle = 2.0 * pow(rng.uniform01(), 1.0 / 3.0) * da;
323  // convert to quaternion
324  double q[4];
325  double norm = sqrt(ax * ax + ay * ay + az * az);
326  if (norm < 1e-6)
327  {
328  q[0] = q[1] = q[2] = 0.0;
329  q[3] = 1.0;
330  }
331  else
332  {
333  double s = sin(angle / 2.0);
334  q[0] = s * ax / norm;
335  q[1] = s * ay / norm;
336  q[2] = s * az / norm;
337  q[3] = cos(angle / 2.0);
338  }
339  // multiply quaternions: near * q
340  values[3] = near[6] * q[0] + near[3] * q[3] + near[4] * q[2] - near[5] * q[1];
341  values[4] = near[6] * q[1] + near[4] * q[3] + near[5] * q[0] - near[3] * q[2];
342  values[5] = near[6] * q[2] + near[5] * q[3] + near[3] * q[1] - near[4] * q[0];
343  values[6] = near[6] * q[3] - near[3] * q[0] - near[4] * q[1] - near[5] * q[2];
344  }
345 }
d
virtual bool satisfiesPositionBounds(const double *values, const Bounds &other_bounds, double margin) const
Check if the set of position values for the variables of this joint are within bounds, up to some margin.
virtual bool enforcePositionBounds(double *values, const Bounds &other_bounds) const
Force the specified values to be inside bounds and normalized. Quaternions are normalized, continuous joints are made between -Pi and Pi. Return true if changes were made.
double distanceRotation(const double *values1, const double *values2) const
Get the distance between the rotation components of two states.
std::vector< std::string > local_variable_names_
The local names to use for the variables that make up this joint.
Definition: joint_model.h:457
double distanceTranslation(const double *values1, const double *values2) const
Get the distance between the translation components of two states.
std::string name_
Name of the joint.
Definition: joint_model.h:451
virtual void computeTransform(const double *joint_values, Eigen::Affine3d &transf) const
Given the joint values for a joint, compute the corresponding transform.
virtual void getVariableDefaultPositions(double *values, const Bounds &other_bounds) const
Provide a default value for the joint given the joint variable bounds. Most joints will use the defau...
virtual unsigned int getStateSpaceDimension() const
Get the dimension of the state space that corresponds to this joint.
XmlRpcServer s
Bounds variable_bounds_
The bounds for each variable (low, high) in the same order as variable_names_.
Definition: joint_model.h:463
FloatingJointModel(const std::string &name)
JointType type_
The type of joint.
Definition: joint_model.h:454
double getMaximumExtent() const
Definition: joint_model.h:431
virtual void computeVariablePositions(const Eigen::Affine3d &transf, double *joint_values) const
Given the transform generated by joint, compute the corresponding joint values.
virtual void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, double *values, const Bounds &other_bounds, const double *near, const double distance) const
Provide random values for the joint variables (within specified bounds). Enough memory is assumed to ...
virtual void interpolate(const double *from, const double *to, const double t, double *state) const
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
double uniformReal(double lower_bound, double upper_bound)
VariableIndexMap variable_index_map_
Map from variable names to the corresponding index in variable_names_ (indexing makes sense within th...
Definition: joint_model.h:469
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
virtual double distance(const double *values1, const double *values2) const
Compute the distance between two joint states of the same model (represented by the variable values) ...
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
Definition: joint_model.h:108
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
std::vector< std::string > variable_names_
The full names to use for the variables that make up this joint.
Definition: joint_model.h:460
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
std::vector< VariableBounds > Bounds
The datatype for the joint bounds.
Definition: joint_model.h:123
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
virtual void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values, const Bounds &other_bounds) const
Provide random values for the joint variables (within specified bounds). Enough memory is assumed to ...
bool normalizeRotation(double *values) const


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jan 15 2018 03:50:44