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