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 
39 #include <boost/math/constants/constants.hpp>
40 #include <algorithm>
41 #include <limits>
42 #include <cmath>
43 
44 namespace moveit
45 {
46 namespace core
47 {
49  : JointModel(name)
50  , axis_(0.0, 0.0, 0.0)
51  , continuous_(false)
52  , x2_(0.0)
53  , y2_(0.0)
54  , z2_(0.0)
55  , xy_(0.0)
56  , xz_(0.0)
57  , yz_(0.0)
58 {
59  type_ = REVOLUTE;
60  variable_names_.push_back(name_);
61  variable_bounds_.resize(1);
62  variable_bounds_[0].position_bounded_ = true;
63  variable_bounds_[0].min_position_ = -boost::math::constants::pi<double>();
64  variable_bounds_[0].max_position_ = boost::math::constants::pi<double>();
67 }
68 
70 {
71  return 1;
72 }
73 
75 {
76  axis_ = axis.normalized();
77  x2_ = axis_.x() * axis_.x();
78  y2_ = axis_.y() * axis_.y();
79  z2_ = axis_.z() * axis_.z();
80  xy_ = axis_.x() * axis_.y();
81  xz_ = axis_.x() * axis_.z();
82  yz_ = axis_.y() * axis_.z();
83 }
84 
86 {
87  continuous_ = flag;
88  if (flag)
89  {
90  variable_bounds_[0].position_bounded_ = false;
91  variable_bounds_[0].min_position_ = -boost::math::constants::pi<double>();
92  variable_bounds_[0].max_position_ = boost::math::constants::pi<double>();
93  }
94  else
95  variable_bounds_[0].position_bounded_ = true;
97 }
98 
99 double RevoluteJointModel::getMaximumExtent(const Bounds& other_bounds) const
100 {
101  return variable_bounds_[0].max_position_ - variable_bounds_[0].min_position_;
102 }
103 
104 void RevoluteJointModel::getVariableDefaultPositions(double* values, const Bounds& bounds) const
105 {
106  // if zero is a valid value
107  if (bounds[0].min_position_ <= 0.0 && bounds[0].max_position_ >= 0.0)
108  values[0] = 0.0;
109  else
110  values[0] = (bounds[0].min_position_ + bounds[0].max_position_) / 2.0;
111 }
112 
114  const Bounds& bounds) const
115 {
116  values[0] = rng.uniformReal(bounds[0].min_position_, bounds[0].max_position_);
117 }
118 
120  const Bounds& bounds, const double* near,
121  const double distance) const
122 {
123  if (continuous_)
124  {
125  values[0] = rng.uniformReal(near[0] - distance, near[0] + distance);
126  enforcePositionBounds(values, bounds);
127  }
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 }
132 
133 void RevoluteJointModel::interpolate(const double* from, const double* to, const double t, double* state) const
134 {
135  if (continuous_)
136  {
137  double diff = to[0] - from[0];
138  if (fabs(diff) <= boost::math::constants::pi<double>())
139  state[0] = from[0] + diff * t;
140  else
141  {
142  if (diff > 0.0)
143  diff = 2.0 * boost::math::constants::pi<double>() - diff;
144  else
145  diff = -2.0 * boost::math::constants::pi<double>() - diff;
146  state[0] = from[0] - diff * t;
147  // input states are within bounds, so the following check is sufficient
148  if (state[0] > boost::math::constants::pi<double>())
149  state[0] -= 2.0 * boost::math::constants::pi<double>();
150  else if (state[0] < -boost::math::constants::pi<double>())
151  state[0] += 2.0 * boost::math::constants::pi<double>();
152  }
153  }
154  else
155  state[0] = from[0] + (to[0] - from[0]) * t;
156 }
157 
158 double RevoluteJointModel::distance(const double* values1, const double* values2) const
159 {
160  if (continuous_)
161  {
162  double d = fmod(fabs(values1[0] - values2[0]), 2.0 * boost::math::constants::pi<double>());
163  return (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
164  }
165  else
166  return fabs(values1[0] - values2[0]);
167 }
168 
169 bool RevoluteJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds, double margin) const
170 {
171  if (continuous_)
172  return true;
173  else
174  return !(values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin);
175 }
176 
177 bool RevoluteJointModel::harmonizePosition(double* values, const JointModel::Bounds& other_bounds) const
178 {
179  bool modified = false;
180  if (*values < other_bounds[0].min_position_)
181  {
182  while (*values + 2 * M_PI <= other_bounds[0].max_position_)
183  {
184  *values += 2 * M_PI;
185  modified = true;
186  }
187  }
188  else if (*values > other_bounds[0].max_position_)
189  {
190  while (*values - 2 * M_PI >= other_bounds[0].min_position_)
191  {
192  *values -= 2 * M_PI;
193  modified = true;
194  }
195  }
196  return modified;
197 }
198 
199 bool RevoluteJointModel::enforcePositionBounds(double* values, const Bounds& bounds) const
200 {
201  if (continuous_)
202  {
203  double& v = values[0];
204  if (v <= -boost::math::constants::pi<double>() || v > boost::math::constants::pi<double>())
205  {
206  v = fmod(v, 2.0 * boost::math::constants::pi<double>());
207  if (v <= -boost::math::constants::pi<double>())
208  v += 2.0 * boost::math::constants::pi<double>();
209  else if (v > boost::math::constants::pi<double>())
210  v -= 2.0 * boost::math::constants::pi<double>();
211  return true;
212  }
213  }
214  else
215  {
216  if (values[0] < bounds[0].min_position_)
217  {
218  values[0] = bounds[0].min_position_;
219  return true;
220  }
221  else if (values[0] > bounds[0].max_position_)
222  {
223  values[0] = bounds[0].max_position_;
224  return true;
225  }
226  }
227  return false;
228 }
229 
230 void RevoluteJointModel::computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const
231 {
232  const double c = cos(joint_values[0]);
233  const double s = sin(joint_values[0]);
234  const double t = 1.0 - c;
235  const double txy = t * xy_;
236  const double txz = t * xz_;
237  const double tyz = t * yz_;
238 
239  const double zs = axis_.z() * s;
240  const double ys = axis_.y() * s;
241  const double xs = axis_.x() * s;
242 
243  // column major
244  double* d = transf.data();
245 
246  d[0] = t * x2_ + c;
247  d[1] = txy + zs;
248  d[2] = txz - ys;
249  d[3] = 0.0;
250 
251  d[4] = txy - zs;
252  d[5] = t * y2_ + c;
253  d[6] = tyz + xs;
254  d[7] = 0.0;
255 
256  d[8] = txz + ys;
257  d[9] = tyz - xs;
258  d[10] = t * z2_ + c;
259  d[11] = 0.0;
260 
261  d[12] = 0.0;
262  d[13] = 0.0;
263  d[14] = 0.0;
264  d[15] = 1.0;
265 
266  // transf = Eigen::Isometry3d(Eigen::AngleAxisd(joint_values[0], axis_));
267 }
268 
269 void RevoluteJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const
270 {
271  Eigen::Quaterniond q(transf.rotation());
272  q.normalize();
273  size_t max_idx;
274  axis_.array().abs().maxCoeff(&max_idx);
275  joint_values[0] = 2. * atan2(q.vec()[max_idx] / axis_[max_idx], q.w());
276 }
277 
278 } // end of namespace core
279 } // end of namespace moveit
d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:90
bool enforcePositionBounds(double *values, const Bounds &other_bounds) const override
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.
bool continuous_
Flag indicating whether this joint wraps around.
void computeTransform(const double *joint_values, Eigen::Isometry3d &transf) const override
Given the joint values for a joint, compute the corresponding transform.
unsigned int getStateSpaceDimension() const override
Get the dimension of the state space that corresponds to this joint.
std::string name_
Name of the joint.
Definition: joint_model.h:459
XmlRpcServer s
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) ...
Bounds variable_bounds_
The bounds for each variable (low, high) in the same order as variable_names_.
Definition: joint_model.h:471
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
JointType type_
The type of joint.
Definition: joint_model.h:462
#define M_PI
geometry_msgs::TransformStamped t
EIGEN_MAKE_ALIGNED_OPERATOR_NEW RevoluteJointModel(const std::string &name)
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 computeVariablePositions(const Eigen::Isometry3d &transf, double *joint_values) const override
Given the transform generated by joint, compute the corresponding joint values.
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...
void setAxis(const Eigen::Vector3d &axis)
Set the axis of rotation.
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:477
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
Definition: joint_model.h:108
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 ...
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, up to some margin.
std::vector< std::string > variable_names_
The full names to use for the variables that make up this joint.
Definition: joint_model.h:468
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
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 ...
bool harmonizePosition(double *values, const Bounds &other_bounds) const override
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Main namespace for MoveIt!
double getMaximumExtent() const
Definition: joint_model.h:439
Eigen::Vector3d axis_
The axis of the joint.
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)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Mar 26 2020 03:51:41