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 
74 void RevoluteJointModel::setAxis(const Eigen::Vector3d& axis)
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::enforcePositionBounds(double* values, const Bounds& bounds) const
178 {
179  if (continuous_)
180  {
181  double& v = values[0];
182  if (v <= -boost::math::constants::pi<double>() || v > boost::math::constants::pi<double>())
183  {
184  v = fmod(v, 2.0 * boost::math::constants::pi<double>());
185  if (v <= -boost::math::constants::pi<double>())
186  v += 2.0 * boost::math::constants::pi<double>();
187  else if (v > boost::math::constants::pi<double>())
188  v -= 2.0 * boost::math::constants::pi<double>();
189  return true;
190  }
191  }
192  else
193  {
194  if (values[0] < bounds[0].min_position_)
195  {
196  values[0] = bounds[0].min_position_;
197  return true;
198  }
199  else if (values[0] > bounds[0].max_position_)
200  {
201  values[0] = bounds[0].max_position_;
202  return true;
203  }
204  }
205  return false;
206 }
207 
208 void RevoluteJointModel::computeTransform(const double* joint_values, Eigen::Affine3d& transf) const
209 {
210  const double c = cos(joint_values[0]);
211  const double s = sin(joint_values[0]);
212  const double t = 1.0 - c;
213  const double txy = t * xy_;
214  const double txz = t * xz_;
215  const double tyz = t * yz_;
216 
217  const double zs = axis_.z() * s;
218  const double ys = axis_.y() * s;
219  const double xs = axis_.x() * s;
220 
221  // column major
222  double* d = transf.data();
223 
224  d[0] = t * x2_ + c;
225  d[1] = txy + zs;
226  d[2] = txz - ys;
227  d[3] = 0.0;
228 
229  d[4] = txy - zs;
230  d[5] = t * y2_ + c;
231  d[6] = tyz + xs;
232  d[7] = 0.0;
233 
234  d[8] = txz + ys;
235  d[9] = tyz - xs;
236  d[10] = t * z2_ + c;
237  d[11] = 0.0;
238 
239  d[12] = 0.0;
240  d[13] = 0.0;
241  d[14] = 0.0;
242  d[15] = 1.0;
243 
244  // transf = Eigen::Affine3d(Eigen::AngleAxisd(joint_values[0], axis_));
245 }
246 
247 void RevoluteJointModel::computeVariablePositions(const Eigen::Affine3d& transf, double* joint_values) const
248 {
249  Eigen::Quaterniond q(transf.linear());
250  q.normalize();
251  size_t maxIdx;
252  axis_.array().abs().maxCoeff(&maxIdx);
253  joint_values[0] = 2. * atan2(q.vec()[maxIdx] / axis_[maxIdx], q.w());
254 }
255 
256 } // end of namespace core
257 } // end of namespace moveit
d
bool continuous_
Flag indicating whether this joint wraps around.
std::string name_
Name of the joint.
Definition: joint_model.h:451
XmlRpcServer s
virtual unsigned int getStateSpaceDimension() const
Get the dimension of the state space that corresponds to this joint.
Bounds variable_bounds_
The bounds for each variable (low, high) in the same order as variable_names_.
Definition: joint_model.h:463
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 ...
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
virtual void computeVariablePositions(const Eigen::Affine3d &transf, double *joint_values) const
Given the transform generated by joint, compute the corresponding joint values.
JointType type_
The type of joint.
Definition: joint_model.h:454
double getMaximumExtent() const
Definition: joint_model.h:431
EIGEN_MAKE_ALIGNED_OPERATOR_NEW RevoluteJointModel(const std::string &name)
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.
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:469
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.
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
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 > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
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...
Main namespace for MoveIt!
virtual void computeTransform(const double *joint_values, Eigen::Affine3d &transf) const
Given the joint values for a joint, compute the corresponding transform.
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)
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 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 ...


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 10 2019 04:03:05