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 
45  : JointModel(name)
46  , axis_(0.0, 0.0, 0.0)
47  , continuous_(false)
48  , x2_(0.0)
49  , y2_(0.0)
50  , z2_(0.0)
51  , xy_(0.0)
52  , xz_(0.0)
53  , yz_(0.0)
54 {
55  type_ = REVOLUTE;
56  variable_names_.push_back(name_);
57  variable_bounds_.resize(1);
58  variable_bounds_[0].position_bounded_ = true;
59  variable_bounds_[0].min_position_ = -boost::math::constants::pi<double>();
60  variable_bounds_[0].max_position_ = boost::math::constants::pi<double>();
63 }
64 
66 {
67  return 1;
68 }
69 
70 void moveit::core::RevoluteJointModel::setAxis(const Eigen::Vector3d& axis)
71 {
72  axis_ = axis.normalized();
73  x2_ = axis_.x() * axis_.x();
74  y2_ = axis_.y() * axis_.y();
75  z2_ = axis_.z() * axis_.z();
76  xy_ = axis_.x() * axis_.y();
77  xz_ = axis_.x() * axis_.z();
78  yz_ = axis_.y() * axis_.z();
79 }
80 
82 {
83  continuous_ = flag;
84  if (flag)
85  {
86  variable_bounds_[0].position_bounded_ = false;
87  variable_bounds_[0].min_position_ = -boost::math::constants::pi<double>();
88  variable_bounds_[0].max_position_ = boost::math::constants::pi<double>();
89  }
90  else
91  variable_bounds_[0].position_bounded_ = true;
93 }
94 
96 {
97  return variable_bounds_[0].max_position_ - variable_bounds_[0].min_position_;
98 }
99 
101 {
102  // if zero is a valid value
103  if (bounds[0].min_position_ <= 0.0 && bounds[0].max_position_ >= 0.0)
104  values[0] = 0.0;
105  else
106  values[0] = (bounds[0].min_position_ + bounds[0].max_position_) / 2.0;
107 }
108 
110  double* values, const Bounds& bounds) const
111 {
112  values[0] = rng.uniformReal(bounds[0].min_position_, bounds[0].max_position_);
113 }
114 
116  double* values, const Bounds& bounds,
117  const double* near, const double distance) const
118 {
119  if (continuous_)
120  {
121  values[0] = rng.uniformReal(near[0] - distance, near[0] + distance);
122  enforcePositionBounds(values, bounds);
123  }
124  else
125  values[0] = rng.uniformReal(std::max(bounds[0].min_position_, near[0] - distance),
126  std::min(bounds[0].max_position_, near[0] + distance));
127 }
128 
129 void moveit::core::RevoluteJointModel::interpolate(const double* from, const double* to, const double t,
130  double* state) const
131 {
132  if (continuous_)
133  {
134  double diff = to[0] - from[0];
135  if (fabs(diff) <= boost::math::constants::pi<double>())
136  state[0] = from[0] + diff * t;
137  else
138  {
139  if (diff > 0.0)
140  diff = 2.0 * boost::math::constants::pi<double>() - diff;
141  else
142  diff = -2.0 * boost::math::constants::pi<double>() - diff;
143  state[0] = from[0] - diff * t;
144  // input states are within bounds, so the following check is sufficient
145  if (state[0] > boost::math::constants::pi<double>())
146  state[0] -= 2.0 * boost::math::constants::pi<double>();
147  else if (state[0] < -boost::math::constants::pi<double>())
148  state[0] += 2.0 * boost::math::constants::pi<double>();
149  }
150  }
151  else
152  state[0] = from[0] + (to[0] - from[0]) * t;
153 }
154 
155 double moveit::core::RevoluteJointModel::distance(const double* values1, const double* values2) const
156 {
157  if (continuous_)
158  {
159  double d = fmod(fabs(values1[0] - values2[0]), 2.0 * boost::math::constants::pi<double>());
160  return (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
161  }
162  else
163  return fabs(values1[0] - values2[0]);
164 }
165 
166 bool moveit::core::RevoluteJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds,
167  double margin) const
168 {
169  if (values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin)
170  return false;
171  return true;
172 }
173 
174 bool moveit::core::RevoluteJointModel::enforcePositionBounds(double* values, const Bounds& bounds) const
175 {
176  if (continuous_)
177  {
178  double& v = values[0];
179  if (v <= -boost::math::constants::pi<double>() || v > boost::math::constants::pi<double>())
180  {
181  v = fmod(v, 2.0 * boost::math::constants::pi<double>());
182  if (v <= -boost::math::constants::pi<double>())
183  v += 2.0 * boost::math::constants::pi<double>();
184  else if (v > boost::math::constants::pi<double>())
185  v -= 2.0 * boost::math::constants::pi<double>();
186  return true;
187  }
188  }
189  else
190  {
191  if (values[0] < bounds[0].min_position_)
192  {
193  values[0] = bounds[0].min_position_;
194  return true;
195  }
196  else if (values[0] > bounds[0].max_position_)
197  {
198  values[0] = bounds[0].max_position_;
199  return true;
200  }
201  }
202  return false;
203 }
204 
205 void moveit::core::RevoluteJointModel::computeTransform(const double* joint_values, Eigen::Affine3d& transf) const
206 {
207  const double c = cos(joint_values[0]);
208  const double s = sin(joint_values[0]);
209  const double t = 1.0 - c;
210  const double txy = t * xy_;
211  const double txz = t * xz_;
212  const double tyz = t * yz_;
213 
214  const double zs = axis_.z() * s;
215  const double ys = axis_.y() * s;
216  const double xs = axis_.x() * s;
217 
218  // column major
219  double* d = transf.data();
220 
221  d[0] = t * x2_ + c;
222  d[1] = txy + zs;
223  d[2] = txz - ys;
224  d[3] = 0.0;
225 
226  d[4] = txy - zs;
227  d[5] = t * y2_ + c;
228  d[6] = tyz + xs;
229  d[7] = 0.0;
230 
231  d[8] = txz + ys;
232  d[9] = tyz - xs;
233  d[10] = t * z2_ + c;
234  d[11] = 0.0;
235 
236  d[12] = 0.0;
237  d[13] = 0.0;
238  d[14] = 0.0;
239  d[15] = 1.0;
240 
241  // transf = Eigen::Affine3d(Eigen::AngleAxisd(joint_values[0], axis_));
242 }
243 
245  double* joint_values) const
246 {
247  Eigen::Quaterniond q(transf.rotation());
248  q.normalize();
249  size_t maxIdx;
250  axis_.array().abs().maxCoeff(&maxIdx);
251  joint_values[0] = 2. * atan2(q.vec()[maxIdx] / axis_[maxIdx], q.w());
252 }
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...
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 Mon Jan 15 2018 03:50:44