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 moveit::core::JointModel::JointModel(const std::string& name)
44  : name_(name)
45  , type_(UNKNOWN)
46  , parent_link_model_(NULL)
47  , child_link_model_(NULL)
48  , mimic_(NULL)
49  , mimic_factor_(1.0)
50  , mimic_offset_(0.0)
51  , passive_(false)
52  , distance_factor_(1.0)
53  , first_variable_index_(-1)
54  , joint_index_(-1)
55 {
56 }
57 
59 {
60 }
61 
63 {
64  switch (type_)
65  {
66  case UNKNOWN:
67  return "Unkown";
68  case REVOLUTE:
69  return "Revolute";
70  case PRISMATIC:
71  return "Prismatic";
72  case PLANAR:
73  return "Planar";
74  case FLOATING:
75  return "Floating";
76  case FIXED:
77  return "Fixed";
78  default:
79  return "[Unkown]";
80  }
81 }
82 
83 int moveit::core::JointModel::getLocalVariableIndex(const std::string& variable) const
84 {
85  VariableIndexMap::const_iterator it = variable_index_map_.find(variable);
86  if (it == variable_index_map_.end())
87  throw Exception("Could not find variable '" + variable + "' to get bounds for within joint '" + name_ + "'");
88  return it->second;
89 }
90 
91 bool moveit::core::JointModel::enforceVelocityBounds(double* values, const Bounds& other_bounds) const
92 {
93  bool change = false;
94  for (std::size_t i = 0; i < other_bounds.size(); ++i)
95  if (other_bounds[i].max_velocity_ < values[i])
96  {
97  values[i] = other_bounds[i].max_velocity_;
98  change = true;
99  }
100  else if (other_bounds[i].min_velocity_ > values[i])
101  {
102  values[i] = other_bounds[i].min_velocity_;
103  change = true;
104  }
105  return change;
106 }
107 
108 bool moveit::core::JointModel::satisfiesVelocityBounds(const double* values, const Bounds& other_bounds,
109  double margin) const
110 {
111  for (std::size_t i = 0; i < other_bounds.size(); ++i)
112  if (other_bounds[i].max_velocity_ + margin < values[i])
113  return false;
114  else if (other_bounds[i].min_velocity_ - margin > values[i])
115  return false;
116  return true;
117 }
118 
120 {
121  return variable_bounds_[getLocalVariableIndex(variable)];
122 }
123 
124 void moveit::core::JointModel::setVariableBounds(const std::string& variable, const VariableBounds& bounds)
125 {
126  variable_bounds_[getLocalVariableIndex(variable)] = bounds;
128 }
129 
130 void moveit::core::JointModel::setVariableBounds(const std::vector<moveit_msgs::JointLimits>& jlim)
131 {
132  for (std::size_t j = 0; j < variable_names_.size(); ++j)
133  for (std::size_t i = 0; i < jlim.size(); ++i)
134  if (jlim[i].joint_name == variable_names_[j])
135  {
136  variable_bounds_[j].position_bounded_ = jlim[i].has_position_limits;
137  if (jlim[i].has_position_limits)
138  {
139  variable_bounds_[j].min_position_ = jlim[i].min_position;
140  variable_bounds_[j].max_position_ = jlim[i].max_position;
141  }
142  variable_bounds_[j].velocity_bounded_ = jlim[i].has_velocity_limits;
143  if (jlim[i].has_velocity_limits)
144  {
145  variable_bounds_[j].min_velocity_ = -jlim[i].max_velocity;
146  variable_bounds_[j].max_velocity_ = jlim[i].max_velocity;
147  }
148  variable_bounds_[j].acceleration_bounded_ = jlim[i].has_acceleration_limits;
149  if (jlim[i].has_acceleration_limits)
150  {
151  variable_bounds_[j].min_acceleration_ = -jlim[i].max_acceleration;
152  variable_bounds_[j].max_acceleration_ = jlim[i].max_acceleration;
153  }
154  break;
155  }
157 }
158 
160 {
161  variable_bounds_msg_.clear();
162  for (std::size_t i = 0; i < variable_bounds_.size(); ++i)
163  {
164  moveit_msgs::JointLimits lim;
165  lim.joint_name = variable_names_[i];
166  lim.has_position_limits = variable_bounds_[i].position_bounded_;
167  lim.min_position = variable_bounds_[i].min_position_;
168  lim.max_position = variable_bounds_[i].max_position_;
169  lim.has_velocity_limits = variable_bounds_[i].velocity_bounded_;
170  lim.max_velocity = std::min(fabs(variable_bounds_[i].min_velocity_), fabs(variable_bounds_[i].max_velocity_));
171  lim.has_acceleration_limits = variable_bounds_[i].acceleration_bounded_;
172  lim.max_acceleration =
173  std::min(fabs(variable_bounds_[i].min_acceleration_), fabs(variable_bounds_[i].max_acceleration_));
174  variable_bounds_msg_.push_back(lim);
175  }
176 }
177 
178 void moveit::core::JointModel::setMimic(const JointModel* mimic, double factor, double offset)
179 {
180  mimic_ = mimic;
181  mimic_factor_ = factor;
182  mimic_offset_ = offset;
183 }
184 
186 {
187  mimic_requests_.push_back(joint);
188 }
189 
191 {
192  descendant_joint_models_.push_back(joint);
193  if (joint->getType() != FIXED)
194  non_fixed_descendant_joint_models_.push_back(joint);
195 }
196 
198 {
199  descendant_link_models_.push_back(link);
200 }
201 
202 namespace
203 {
204 inline void printBoundHelper(std::ostream& out, double v)
205 {
206  if (v <= -std::numeric_limits<double>::infinity())
207  out << "-inf";
208  else if (v >= std::numeric_limits<double>::infinity())
209  out << "inf";
210  else
211  out << v;
212 }
213 }
214 
215 std::ostream& moveit::core::operator<<(std::ostream& out, const VariableBounds& b)
216 {
217  out << "P." << (b.position_bounded_ ? "bounded" : "unbounded") << " [";
218  printBoundHelper(out, b.min_position_);
219  out << ", ";
220  printBoundHelper(out, b.max_position_);
221  out << "]; "
222  << "V." << (b.velocity_bounded_ ? "bounded" : "unbounded") << " [";
223  printBoundHelper(out, b.min_velocity_);
224  out << ", ";
225  printBoundHelper(out, b.max_velocity_);
226  out << "]; "
227  << "A." << (b.acceleration_bounded_ ? "bounded" : "unbounded") << " [";
228  printBoundHelper(out, b.min_acceleration_);
229  out << ", ";
230  printBoundHelper(out, b.max_acceleration_);
231  out << "];";
232  return out;
233 }
JointModel(const std::string &name)
Construct a joint named name.
Definition: joint_model.cpp:43
void setMimic(const JointModel *mimic, double factor, double offset)
Mark this joint as mimicking mimic using factor and offset.
std::string name_
Name of the joint.
Definition: joint_model.h:451
std::vector< const JointModel * > non_fixed_descendant_joint_models_
Pointers to all the joints that follow this one in the kinematic tree, including mimic joints...
Definition: joint_model.h:497
void addDescendantJointModel(const JointModel *joint)
Bounds variable_bounds_
The bounds for each variable (low, high) in the same order as variable_names_.
Definition: joint_model.h:463
const JointModel * mimic_
The joint this one mimics (NULL for joints that do not mimic)
Definition: joint_model.h:478
JointType type_
The type of joint.
Definition: joint_model.h:454
bool enforceVelocityBounds(double *values) const
Force the specified velocities to be within bounds. Return true if changes were made.
Definition: joint_model.h:310
int getLocalVariableIndex(const std::string &variable) const
Get the index of the variable within this joint.
Definition: joint_model.cpp:83
double mimic_offset_
The multiplier to the mimic joint.
Definition: joint_model.h:484
std::vector< const LinkModel * > descendant_link_models_
Pointers to all the links that will be moved if this joint changes value.
Definition: joint_model.h:490
void addMimicRequest(const JointModel *joint)
Notify this joint that there is another joint that mimics it.
This may be thrown if unrecoverable errors occur.
Definition: exceptions.h:53
const Bounds & getVariableBounds() const
Get the variable bounds for this joint, in the same order as the names returned by getVariableNames()...
Definition: joint_model.h:322
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
void addDescendantLinkModel(const LinkModel *link)
double mimic_factor_
The offset to the mimic joint.
Definition: joint_model.h:481
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
Definition: joint_model.h:108
std::vector< const JointModel * > mimic_requests_
The set of joints that should get a value copied to them when this joint changes. ...
Definition: joint_model.h:487
std::vector< moveit_msgs::JointLimits > variable_bounds_msg_
Definition: joint_model.h:465
std::vector< std::string > variable_names_
The full names to use for the variables that make up this joint.
Definition: joint_model.h:460
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
std::vector< const JointModel * > descendant_joint_models_
Pointers to all the joints that follow this one in the kinematic tree (including mimic joints) ...
Definition: joint_model.h:493
std::string getTypeName() const
Get the type of joint as a string.
Definition: joint_model.cpp:62
bool satisfiesVelocityBounds(const double *values, double margin=0.0) const
Check if the set of velocities for the variables of this joint are within bounds. ...
Definition: joint_model.h:301
std::ostream & operator<<(std::ostream &out, const VariableBounds &b)
Operator overload for printing variable bounds to a stream.
std::vector< VariableBounds > Bounds
The datatype for the joint bounds.
Definition: joint_model.h:123
void setVariableBounds(const std::string &variable, const VariableBounds &bounds)
Set the lower and upper bounds for a variable. Throw an exception if the variable was not found...
JointType getType() const
Get the type of joint.
Definition: joint_model.h:137


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jan 15 2018 03:50:44