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 
41 #include <algorithm>
42 
43 namespace moveit
44 {
45 namespace core
46 {
47 JointModel::JointModel(const std::string& name)
48  : name_(name)
49  , type_(UNKNOWN)
50  , parent_link_model_(nullptr)
51  , child_link_model_(nullptr)
52  , mimic_(nullptr)
53  , mimic_factor_(1.0)
54  , mimic_offset_(0.0)
55  , passive_(false)
56  , distance_factor_(1.0)
57  , first_variable_index_(-1)
58  , joint_index_(-1)
59 {
60 }
61 
62 JointModel::~JointModel() = default;
63 
64 std::string JointModel::getTypeName() const
65 {
66  switch (type_)
67  {
68  case UNKNOWN:
69  return "Unkown";
70  case REVOLUTE:
71  return "Revolute";
72  case PRISMATIC:
73  return "Prismatic";
74  case PLANAR:
75  return "Planar";
76  case FLOATING:
77  return "Floating";
78  case FIXED:
79  return "Fixed";
80  default:
81  return "[Unkown]";
82  }
83 }
84 
85 int JointModel::getLocalVariableIndex(const std::string& variable) const
86 {
87  VariableIndexMap::const_iterator it = variable_index_map_.find(variable);
88  if (it == variable_index_map_.end())
89  throw Exception("Could not find variable '" + variable + "' to get bounds for within joint '" + name_ + "'");
90  return it->second;
91 }
92 
93 bool JointModel::harmonizePosition(double* values, const Bounds& other_bounds) const
94 {
95  return false;
96 }
97 
98 bool JointModel::enforceVelocityBounds(double* values, const Bounds& other_bounds) const
99 {
100  bool change = false;
101  for (std::size_t i = 0; i < other_bounds.size(); ++i)
102  if (other_bounds[i].max_velocity_ < values[i])
103  {
104  values[i] = other_bounds[i].max_velocity_;
105  change = true;
106  }
107  else if (other_bounds[i].min_velocity_ > values[i])
108  {
109  values[i] = other_bounds[i].min_velocity_;
110  change = true;
111  }
112  return change;
113 }
114 
115 bool JointModel::satisfiesVelocityBounds(const double* values, const Bounds& other_bounds, double margin) const
116 {
117  for (std::size_t i = 0; i < other_bounds.size(); ++i)
118  if (other_bounds[i].max_velocity_ + margin < values[i])
119  return false;
120  else if (other_bounds[i].min_velocity_ - margin > values[i])
121  return false;
122  return true;
123 }
124 
125 const VariableBounds& JointModel::getVariableBounds(const std::string& variable) const
126 {
127  return variable_bounds_[getLocalVariableIndex(variable)];
128 }
129 
130 void JointModel::setVariableBounds(const std::string& variable, const VariableBounds& bounds)
131 {
132  variable_bounds_[getLocalVariableIndex(variable)] = bounds;
133  computeVariableBoundsMsg();
134 }
135 
136 void JointModel::setVariableBounds(const std::vector<moveit_msgs::JointLimits>& jlim)
137 {
138  for (std::size_t j = 0; j < variable_names_.size(); ++j)
139  for (const moveit_msgs::JointLimits& joint_limit : jlim)
140  if (joint_limit.joint_name == variable_names_[j])
141  {
142  variable_bounds_[j].position_bounded_ = joint_limit.has_position_limits;
143  if (joint_limit.has_position_limits)
144  {
145  variable_bounds_[j].min_position_ = joint_limit.min_position;
146  variable_bounds_[j].max_position_ = joint_limit.max_position;
147  }
148  variable_bounds_[j].velocity_bounded_ = joint_limit.has_velocity_limits;
149  if (joint_limit.has_velocity_limits)
150  {
151  variable_bounds_[j].min_velocity_ = -joint_limit.max_velocity;
152  variable_bounds_[j].max_velocity_ = joint_limit.max_velocity;
153  }
154  variable_bounds_[j].acceleration_bounded_ = joint_limit.has_acceleration_limits;
155  if (joint_limit.has_acceleration_limits)
156  {
157  variable_bounds_[j].min_acceleration_ = -joint_limit.max_acceleration;
158  variable_bounds_[j].max_acceleration_ = joint_limit.max_acceleration;
159  }
160  break;
161  }
162  computeVariableBoundsMsg();
163 }
164 
165 void JointModel::computeVariableBoundsMsg()
166 {
167  variable_bounds_msg_.clear();
168  for (std::size_t i = 0; i < variable_bounds_.size(); ++i)
169  {
170  moveit_msgs::JointLimits lim;
171  lim.joint_name = variable_names_[i];
172  lim.has_position_limits = variable_bounds_[i].position_bounded_;
173  lim.min_position = variable_bounds_[i].min_position_;
174  lim.max_position = variable_bounds_[i].max_position_;
175  lim.has_velocity_limits = variable_bounds_[i].velocity_bounded_;
176  lim.max_velocity = std::min(fabs(variable_bounds_[i].min_velocity_), fabs(variable_bounds_[i].max_velocity_));
177  lim.has_acceleration_limits = variable_bounds_[i].acceleration_bounded_;
178  lim.max_acceleration =
179  std::min(fabs(variable_bounds_[i].min_acceleration_), fabs(variable_bounds_[i].max_acceleration_));
180  variable_bounds_msg_.push_back(lim);
181  }
182 }
183 
184 void JointModel::setMimic(const JointModel* mimic, double factor, double offset)
185 {
186  mimic_ = mimic;
187  mimic_factor_ = factor;
188  mimic_offset_ = offset;
189 }
190 
191 void JointModel::addMimicRequest(const JointModel* joint)
192 {
193  mimic_requests_.push_back(joint);
194 }
195 
196 void JointModel::addDescendantJointModel(const JointModel* joint)
197 {
198  descendant_joint_models_.push_back(joint);
199  if (joint->getType() != FIXED)
200  non_fixed_descendant_joint_models_.push_back(joint);
201 }
202 
203 void JointModel::addDescendantLinkModel(const LinkModel* link)
204 {
205  descendant_link_models_.push_back(link);
206 }
207 
208 namespace
209 {
210 inline void printBoundHelper(std::ostream& out, double v)
211 {
212  if (v <= -std::numeric_limits<double>::infinity())
213  out << "-inf";
214  else if (v >= std::numeric_limits<double>::infinity())
215  out << "inf";
216  else
217  out << v;
218 }
219 } // namespace
220 
221 std::ostream& operator<<(std::ostream& out, const VariableBounds& b)
222 {
223  out << "P." << (b.position_bounded_ ? "bounded" : "unbounded") << " [";
224  printBoundHelper(out, b.min_position_);
225  out << ", ";
226  printBoundHelper(out, b.max_position_);
227  out << "]; "
228  << "V." << (b.velocity_bounded_ ? "bounded" : "unbounded") << " [";
229  printBoundHelper(out, b.min_velocity_);
230  out << ", ";
231  printBoundHelper(out, b.max_velocity_);
232  out << "]; "
233  << "A." << (b.acceleration_bounded_ ? "bounded" : "unbounded") << " [";
234  printBoundHelper(out, b.min_acceleration_);
235  out << ", ";
236  printBoundHelper(out, b.max_acceleration_);
237  out << "];";
238  return out;
239 }
240 
241 } // end of namespace core
242 } // end of namespace moveit
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:71
exceptions.h
moveit::core::operator<<
std::ostream & operator<<(std::ostream &out, const VariableBounds &b)
Operator overload for printing variable bounds to a stream.
Definition: joint_model.cpp:287
moveit::core::VariableBounds
Definition: joint_model.h:118
moveit::core::JointModel::getType
JointType getType() const
Get the type of joint.
Definition: joint_model.h:202
moveit
Main namespace for MoveIt.
Definition: background_processing.h:46
values
std::vector< double > values
moveit::core::JointModel::JointModel
JointModel(const std::string &name)
Construct a joint named name.
Definition: joint_model.cpp:113
joint_model.h
moveit::core::JointModel
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:173


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Oct 21 2020 03:28:04