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