prismatic_joint_model.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
38 #include <limits>
39 
40 namespace moveit
41 {
42 namespace core
43 {
44 PrismaticJointModel::PrismaticJointModel(const std::string& name) : JointModel(name), axis_(0.0, 0.0, 0.0)
45 {
46  type_ = PRISMATIC;
47  variable_names_.push_back(name_);
48  variable_bounds_.resize(1);
49  variable_bounds_[0].position_bounded_ = true;
50  variable_bounds_[0].min_position_ = -std::numeric_limits<double>::max();
51  variable_bounds_[0].max_position_ = std::numeric_limits<double>::max();
52  variable_index_map_[name_] = 0;
53  computeVariableBoundsMsg();
54 }
55 
56 unsigned int PrismaticJointModel::getStateSpaceDimension() const
57 {
58  return 1;
59 }
60 
61 double PrismaticJointModel::getMaximumExtent(const Bounds& other_bounds) const
62 {
63  return variable_bounds_[0].max_position_ - other_bounds[0].min_position_;
64 }
65 
66 void PrismaticJointModel::getVariableDefaultPositions(double* values, const Bounds& bounds) const
67 {
68  // if zero is a valid value
69  if (bounds[0].min_position_ <= 0.0 && bounds[0].max_position_ >= 0.0)
70  values[0] = 0.0;
71  else
72  values[0] = (bounds[0].min_position_ + bounds[0].max_position_) / 2.0;
73 }
74 
75 bool PrismaticJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds, double margin) const
76 {
77  return !(values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin);
78 }
79 
80 void PrismaticJointModel::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values,
81  const Bounds& bounds) const
82 {
83  values[0] = rng.uniformReal(bounds[0].min_position_, bounds[0].max_position_);
84 }
85 
86 void PrismaticJointModel::getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values,
87  const Bounds& bounds, const double* seed,
88  const double distance) const
89 {
90  values[0] = rng.uniformReal(std::max(bounds[0].min_position_, seed[0] - distance),
91  std::min(bounds[0].max_position_, seed[0] + distance));
92 }
93 
94 bool PrismaticJointModel::enforcePositionBounds(double* values, const Bounds& bounds) const
95 {
96  if (values[0] < bounds[0].min_position_)
97  {
98  values[0] = bounds[0].min_position_;
99  return true;
100  }
101  else if (values[0] > bounds[0].max_position_)
102  {
103  values[0] = bounds[0].max_position_;
104  return true;
105  }
106  return false;
107 }
108 
109 double PrismaticJointModel::distance(const double* values1, const double* values2) const
110 {
111  return fabs(values1[0] - values2[0]);
112 }
113 
114 void PrismaticJointModel::interpolate(const double* from, const double* to, const double t, double* state) const
115 {
116  state[0] = from[0] + (to[0] - from[0]) * t;
117 }
118 
119 void PrismaticJointModel::computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const
120 {
121  double* d = transf.data();
122  d[0] = 1.0;
123  d[1] = 0.0;
124  d[2] = 0.0;
125  d[3] = 0.0;
126 
127  d[4] = 0.0;
128  d[5] = 1.0;
129  d[6] = 0.0;
130  d[7] = 0.0;
131 
132  d[8] = 0.0;
133  d[9] = 0.0;
134  d[10] = 1.0;
135  d[11] = 0.0;
136 
137  d[12] = axis_.x() * joint_values[0];
138  d[13] = axis_.y() * joint_values[0];
139  d[14] = axis_.z() * joint_values[0];
140  d[15] = 1.0;
141 
142  // transf.setIdentity();
143  // transf.translation() = Eigen::Vector3d(axis_ * joint_values[0]);
144 }
145 
146 void PrismaticJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const
147 {
148  joint_values[0] = transf.translation().dot(axis_);
149 }
150 
151 } // end of namespace core
152 } // end of namespace moveit
name_
std::string name_
random_numbers::RandomNumberGenerator::uniformReal
double uniformReal(double lower_bound, double upper_bound)
name
std::string name
d
d
random_numbers::RandomNumberGenerator
moveit::core::PrismaticJointModel::PrismaticJointModel
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PrismaticJointModel(const std::string &name)
Definition: prismatic_joint_model.cpp:108
moveit
Main namespace for MoveIt.
Definition: background_processing.h:46
values
std::vector< double > values
pr2_arm_kinematics::distance
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:86
prismatic_joint_model.h


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