revolute_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 <algorithm>
42 #include <cmath>
43 
44 namespace moveit
45 {
46 namespace core
47 {
48 RevoluteJointModel::RevoluteJointModel(const std::string& name)
49  : JointModel(name), axis_(0.0, 0.0, 0.0), continuous_(false), x2_(0.0), y2_(0.0), z2_(0.0), xy_(0.0), xz_(0.0), yz_(0.0)
50 {
51  type_ = REVOLUTE;
52  variable_names_.push_back(name_);
53  variable_bounds_.resize(1);
54  variable_bounds_[0].position_bounded_ = true;
55  variable_bounds_[0].min_position_ = -boost::math::constants::pi<double>();
56  variable_bounds_[0].max_position_ = boost::math::constants::pi<double>();
57  variable_index_map_[name_] = 0;
58  computeVariableBoundsMsg();
59 }
60 
61 unsigned int RevoluteJointModel::getStateSpaceDimension() const
62 {
63  return 1;
64 }
65 
66 void RevoluteJointModel::setAxis(const Eigen::Vector3d& axis)
67 {
68  axis_ = axis.normalized();
69  x2_ = axis_.x() * axis_.x();
70  y2_ = axis_.y() * axis_.y();
71  z2_ = axis_.z() * axis_.z();
72  xy_ = axis_.x() * axis_.y();
73  xz_ = axis_.x() * axis_.z();
74  yz_ = axis_.y() * axis_.z();
75 }
76 
77 void RevoluteJointModel::setContinuous(bool flag)
78 {
79  continuous_ = flag;
80  if (flag)
81  {
82  variable_bounds_[0].position_bounded_ = false;
83  variable_bounds_[0].min_position_ = -boost::math::constants::pi<double>();
84  variable_bounds_[0].max_position_ = boost::math::constants::pi<double>();
85  }
86  else
87  variable_bounds_[0].position_bounded_ = true;
88  computeVariableBoundsMsg();
89 }
90 
91 double RevoluteJointModel::getMaximumExtent(const Bounds& /*other_bounds*/) const
92 {
93  return variable_bounds_[0].max_position_ - variable_bounds_[0].min_position_;
94 }
95 
96 void RevoluteJointModel::getVariableDefaultPositions(double* values, const Bounds& bounds) const
97 {
98  // if zero is a valid value
99  if (bounds[0].min_position_ <= 0.0 && bounds[0].max_position_ >= 0.0)
100  values[0] = 0.0;
101  else
102  values[0] = (bounds[0].min_position_ + bounds[0].max_position_) / 2.0;
103 }
104 
105 void RevoluteJointModel::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values,
106  const Bounds& bounds) const
107 {
108  values[0] = rng.uniformReal(bounds[0].min_position_, bounds[0].max_position_);
109 }
110 
111 void RevoluteJointModel::getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values,
112  const Bounds& bounds, const double* seed,
113  const double distance) const
114 {
115  if (continuous_)
116  {
117  values[0] = rng.uniformReal(seed[0] - distance, seed[0] + distance);
118  enforcePositionBounds(values, bounds);
119  }
120  else
121  values[0] = rng.uniformReal(std::max(bounds[0].min_position_, seed[0] - distance),
122  std::min(bounds[0].max_position_, seed[0] + distance));
123 }
124 
125 void RevoluteJointModel::interpolate(const double* from, const double* to, const double t, double* state) const
126 {
127  if (continuous_)
128  {
129  double diff = to[0] - from[0];
130  if (fabs(diff) <= boost::math::constants::pi<double>())
131  state[0] = from[0] + diff * t;
132  else
133  {
134  if (diff > 0.0)
135  diff = 2.0 * boost::math::constants::pi<double>() - diff;
136  else
137  diff = -2.0 * boost::math::constants::pi<double>() - diff;
138  state[0] = from[0] - diff * t;
139  // input states are within bounds, so the following check is sufficient
140  if (state[0] > boost::math::constants::pi<double>())
141  state[0] -= 2.0 * boost::math::constants::pi<double>();
142  else if (state[0] < -boost::math::constants::pi<double>())
143  state[0] += 2.0 * boost::math::constants::pi<double>();
144  }
145  }
146  else
147  state[0] = from[0] + (to[0] - from[0]) * t;
148 }
149 
150 double RevoluteJointModel::distance(const double* values1, const double* values2) const
151 {
152  if (continuous_)
153  {
154  double d = fmod(fabs(values1[0] - values2[0]), 2.0 * boost::math::constants::pi<double>());
155  return (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
156  }
157  else
158  return fabs(values1[0] - values2[0]);
159 }
160 
161 bool RevoluteJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds, double margin) const
162 {
163  if (continuous_)
164  return true;
165  else
166  return !(values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin);
167 }
168 
169 bool RevoluteJointModel::harmonizePosition(double* values, const JointModel::Bounds& other_bounds) const
170 {
171  bool modified = false;
172  if (*values < other_bounds[0].min_position_)
173  {
174  while (*values + 2 * M_PI <= other_bounds[0].max_position_)
175  {
176  *values += 2 * M_PI;
177  modified = true;
178  }
179  }
180  else if (*values > other_bounds[0].max_position_)
181  {
182  while (*values - 2 * M_PI >= other_bounds[0].min_position_)
183  {
184  *values -= 2 * M_PI;
185  modified = true;
186  }
187  }
188  return modified;
189 }
190 
191 bool RevoluteJointModel::enforcePositionBounds(double* values, const Bounds& bounds) const
192 {
193  if (continuous_)
194  {
195  double& v = values[0];
196  if (v <= -boost::math::constants::pi<double>() || v > boost::math::constants::pi<double>())
197  {
198  v = fmod(v, 2.0 * boost::math::constants::pi<double>());
199  if (v <= -boost::math::constants::pi<double>())
200  v += 2.0 * boost::math::constants::pi<double>();
201  else if (v > boost::math::constants::pi<double>())
202  v -= 2.0 * boost::math::constants::pi<double>();
203  return true;
204  }
205  }
206  else
207  {
208  if (values[0] < bounds[0].min_position_)
209  {
210  values[0] = bounds[0].min_position_;
211  return true;
212  }
213  else if (values[0] > bounds[0].max_position_)
214  {
215  values[0] = bounds[0].max_position_;
216  return true;
217  }
218  }
219  return false;
220 }
221 
222 void RevoluteJointModel::computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const
223 {
224  const double c = cos(joint_values[0]);
225  const double s = sin(joint_values[0]);
226  const double t = 1.0 - c;
227  const double txy = t * xy_;
228  const double txz = t * xz_;
229  const double tyz = t * yz_;
230 
231  const double zs = axis_.z() * s;
232  const double ys = axis_.y() * s;
233  const double xs = axis_.x() * s;
234 
235  // column major
236  double* d = transf.data();
237 
238  d[0] = t * x2_ + c;
239  d[1] = txy + zs;
240  d[2] = txz - ys;
241  d[3] = 0.0;
242 
243  d[4] = txy - zs;
244  d[5] = t * y2_ + c;
245  d[6] = tyz + xs;
246  d[7] = 0.0;
247 
248  d[8] = txz + ys;
249  d[9] = tyz - xs;
250  d[10] = t * z2_ + c;
251  d[11] = 0.0;
252 
253  d[12] = 0.0;
254  d[13] = 0.0;
255  d[14] = 0.0;
256  d[15] = 1.0;
257 
258  // transf = Eigen::Isometry3d(Eigen::AngleAxisd(joint_values[0], axis_));
259 }
260 
261 void RevoluteJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const
262 {
263  ASSERT_ISOMETRY(transf) // unsanitized input, could contain a non-isometry
264  Eigen::Quaterniond q(transf.linear());
265  q.normalize();
266  size_t max_idx;
267  axis_.array().abs().maxCoeff(&max_idx);
268  joint_values[0] = 2. * atan2(q.vec()[max_idx] / axis_[max_idx], q.w());
269 }
270 
271 } // end of namespace core
272 } // end of namespace moveit
name_
std::string name_
s
XmlRpcServer s
check_isometry.h
random_numbers::RandomNumberGenerator::uniformReal
double uniformReal(double lower_bound, double upper_bound)
name
std::string name
moveit::core::JointModel::Bounds
std::vector< VariableBounds > Bounds
The datatype for the joint bounds.
Definition: joint_model.h:188
ASSERT_ISOMETRY
#define ASSERT_ISOMETRY(transform)
d
d
revolute_joint_model.h
random_numbers::RandomNumberGenerator
moveit
Main namespace for MoveIt.
Definition: background_processing.h:46
values
std::vector< double > values
M_PI
#define M_PI
moveit::core::RevoluteJointModel::RevoluteJointModel
EIGEN_MAKE_ALIGNED_OPERATOR_NEW RevoluteJointModel(const std::string &name)
Definition: revolute_joint_model.cpp:114
pr2_arm_kinematics::distance
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:86
t
geometry_msgs::TransformStamped t
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Nov 21 2021 03:25:56