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;
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 (std::size_t i = 0; i < jlim.size(); ++i)
140  if (jlim[i].joint_name == variable_names_[j])
141  {
142  variable_bounds_[j].position_bounded_ = jlim[i].has_position_limits;
143  if (jlim[i].has_position_limits)
144  {
145  variable_bounds_[j].min_position_ = jlim[i].min_position;
146  variable_bounds_[j].max_position_ = jlim[i].max_position;
147  }
148  variable_bounds_[j].velocity_bounded_ = jlim[i].has_velocity_limits;
149  if (jlim[i].has_velocity_limits)
150  {
151  variable_bounds_[j].min_velocity_ = -jlim[i].max_velocity;
152  variable_bounds_[j].max_velocity_ = jlim[i].max_velocity;
153  }
154  variable_bounds_[j].acceleration_bounded_ = jlim[i].has_acceleration_limits;
155  if (jlim[i].has_acceleration_limits)
156  {
157  variable_bounds_[j].min_acceleration_ = -jlim[i].max_acceleration;
158  variable_bounds_[j].max_acceleration_ = jlim[i].max_acceleration;
159  }
160  break;
161  }
163 }
164 
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 
192 {
193  mimic_requests_.push_back(joint);
194 }
195 
197 {
198  descendant_joint_models_.push_back(joint);
199  if (joint->getType() != FIXED)
200  non_fixed_descendant_joint_models_.push_back(joint);
201 }
202 
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
JointModel(const std::string &name)
Construct a joint named name.
Definition: joint_model.cpp:47
std::string getTypeName() const
Get the type of joint as a string.
Definition: joint_model.cpp:64
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:459
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:505
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:471
virtual bool harmonizePosition(double *values, const Bounds &other_bounds) const
Definition: joint_model.cpp:93
const JointModel * mimic_
The joint this one mimics (NULL for joints that do not mimic)
Definition: joint_model.h:486
JointType type_
The type of joint.
Definition: joint_model.h:462
double mimic_offset_
The multiplier to the mimic joint.
Definition: joint_model.h:492
int getLocalVariableIndex(const std::string &variable) const
Get the index of the variable within this joint.
Definition: joint_model.cpp:85
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:309
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:498
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
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
void addDescendantLinkModel(const LinkModel *link)
double mimic_factor_
The offset to the mimic joint.
Definition: joint_model.h:489
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:330
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:495
bool enforceVelocityBounds(double *values) const
Force the specified velocities to be within bounds. Return true if changes were made.
Definition: joint_model.h:318
std::vector< moveit_msgs::JointLimits > variable_bounds_msg_
Definition: joint_model.h:473
std::vector< std::string > variable_names_
The full names to use for the variables that make up this joint.
Definition: joint_model.h:468
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:501
Main namespace for MoveIt!
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 8 2020 03:52:11