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::enforceVelocityBounds(double* values, const Bounds& other_bounds) const
94 {
95  bool change = false;
96  for (std::size_t i = 0; i < other_bounds.size(); ++i)
97  if (other_bounds[i].max_velocity_ < values[i])
98  {
99  values[i] = other_bounds[i].max_velocity_;
100  change = true;
101  }
102  else if (other_bounds[i].min_velocity_ > values[i])
103  {
104  values[i] = other_bounds[i].min_velocity_;
105  change = true;
106  }
107  return change;
108 }
109 
110 bool JointModel::satisfiesVelocityBounds(const double* values, const Bounds& other_bounds, double margin) const
111 {
112  for (std::size_t i = 0; i < other_bounds.size(); ++i)
113  if (other_bounds[i].max_velocity_ + margin < values[i])
114  return false;
115  else if (other_bounds[i].min_velocity_ - margin > values[i])
116  return false;
117  return true;
118 }
119 
120 const VariableBounds& JointModel::getVariableBounds(const std::string& variable) const
121 {
122  return variable_bounds_[getLocalVariableIndex(variable)];
123 }
124 
125 void JointModel::setVariableBounds(const std::string& variable, const VariableBounds& bounds)
126 {
127  variable_bounds_[getLocalVariableIndex(variable)] = bounds;
129 }
130 
131 void JointModel::setVariableBounds(const std::vector<moveit_msgs::JointLimits>& jlim)
132 {
133  for (std::size_t j = 0; j < variable_names_.size(); ++j)
134  for (std::size_t i = 0; i < jlim.size(); ++i)
135  if (jlim[i].joint_name == variable_names_[j])
136  {
137  variable_bounds_[j].position_bounded_ = jlim[i].has_position_limits;
138  if (jlim[i].has_position_limits)
139  {
140  variable_bounds_[j].min_position_ = jlim[i].min_position;
141  variable_bounds_[j].max_position_ = jlim[i].max_position;
142  }
143  variable_bounds_[j].velocity_bounded_ = jlim[i].has_velocity_limits;
144  if (jlim[i].has_velocity_limits)
145  {
146  variable_bounds_[j].min_velocity_ = -jlim[i].max_velocity;
147  variable_bounds_[j].max_velocity_ = jlim[i].max_velocity;
148  }
149  variable_bounds_[j].acceleration_bounded_ = jlim[i].has_acceleration_limits;
150  if (jlim[i].has_acceleration_limits)
151  {
152  variable_bounds_[j].min_acceleration_ = -jlim[i].max_acceleration;
153  variable_bounds_[j].max_acceleration_ = jlim[i].max_acceleration;
154  }
155  break;
156  }
158 }
159 
161 {
162  variable_bounds_msg_.clear();
163  for (std::size_t i = 0; i < variable_bounds_.size(); ++i)
164  {
165  moveit_msgs::JointLimits lim;
166  lim.joint_name = variable_names_[i];
167  lim.has_position_limits = variable_bounds_[i].position_bounded_;
168  lim.min_position = variable_bounds_[i].min_position_;
169  lim.max_position = variable_bounds_[i].max_position_;
170  lim.has_velocity_limits = variable_bounds_[i].velocity_bounded_;
171  lim.max_velocity = std::min(fabs(variable_bounds_[i].min_velocity_), fabs(variable_bounds_[i].max_velocity_));
172  lim.has_acceleration_limits = variable_bounds_[i].acceleration_bounded_;
173  lim.max_acceleration =
174  std::min(fabs(variable_bounds_[i].min_acceleration_), fabs(variable_bounds_[i].max_acceleration_));
175  variable_bounds_msg_.push_back(lim);
176  }
177 }
178 
179 void JointModel::setMimic(const JointModel* mimic, double factor, double offset)
180 {
181  mimic_ = mimic;
182  mimic_factor_ = factor;
183  mimic_offset_ = offset;
184 }
185 
187 {
188  mimic_requests_.push_back(joint);
189 }
190 
192 {
193  descendant_joint_models_.push_back(joint);
194  if (joint->getType() != FIXED)
195  non_fixed_descendant_joint_models_.push_back(joint);
196 }
197 
199 {
200  descendant_link_models_.push_back(link);
201 }
202 
203 namespace
204 {
205 inline void printBoundHelper(std::ostream& out, double v)
206 {
207  if (v <= -std::numeric_limits<double>::infinity())
208  out << "-inf";
209  else if (v >= std::numeric_limits<double>::infinity())
210  out << "inf";
211  else
212  out << v;
213 }
214 }
215 
216 std::ostream& operator<<(std::ostream& out, const VariableBounds& b)
217 {
218  out << "P." << (b.position_bounded_ ? "bounded" : "unbounded") << " [";
219  printBoundHelper(out, b.min_position_);
220  out << ", ";
221  printBoundHelper(out, b.max_position_);
222  out << "]; "
223  << "V." << (b.velocity_bounded_ ? "bounded" : "unbounded") << " [";
224  printBoundHelper(out, b.min_velocity_);
225  out << ", ";
226  printBoundHelper(out, b.max_velocity_);
227  out << "]; "
228  << "A." << (b.acceleration_bounded_ ? "bounded" : "unbounded") << " [";
229  printBoundHelper(out, b.min_acceleration_);
230  out << ", ";
231  printBoundHelper(out, b.max_acceleration_);
232  out << "];";
233  return out;
234 }
235 
236 } // end of namespace core
237 } // end of namespace moveit
JointModel(const std::string &name)
Construct a joint named name.
Definition: joint_model.cpp:47
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:85
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
Main namespace for MoveIt!
std::string getTypeName() const
Get the type of joint as a string.
Definition: joint_model.cpp:64
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 Wed Jul 18 2018 02:48:31