moveit2
The MoveIt Motion Planning Framework for ROS 2.
joint_limits_container.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018 Pilz GmbH & Co. KG
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 Pilz GmbH & Co. KG 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 
36 #include <cmath>
37 
38 #include <rclcpp/logger.hpp>
39 #include <rclcpp/logging.hpp>
40 #include <stdexcept>
41 
43 {
44 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.joint_limits_container");
45 bool JointLimitsContainer::addLimit(const std::string& joint_name,
47 {
48  if (joint_limit.has_deceleration_limits && joint_limit.max_deceleration >= 0)
49  {
50  RCLCPP_ERROR_STREAM(LOGGER, "joint_limit.max_deceleration MUST be negative!");
51  return false;
52  }
53  const auto& insertion_result{ container_.insert(std::pair<std::string, JointLimit>(joint_name, joint_limit)) };
54  if (!insertion_result.second)
55  {
56  RCLCPP_ERROR_STREAM(LOGGER, "joint_limit for joint " << joint_name << " already contained.");
57  return false;
58  }
59  return true;
60 }
61 
62 bool JointLimitsContainer::hasLimit(const std::string& joint_name) const
63 {
64  return container_.find(joint_name) != container_.end();
65 }
66 
68 {
69  return container_.size();
70 }
71 
73 {
74  return container_.empty();
75 }
76 
78 {
80  for (const auto& limit : container_)
81  {
82  updateCommonLimit(limit.second, common_limit);
83  }
84  return common_limit;
85 }
86 
87 JointLimit JointLimitsContainer::getCommonLimit(const std::vector<std::string>& joint_names) const
88 {
90  for (const auto& joint_name : joint_names)
91  {
92  updateCommonLimit(container_.at(joint_name), common_limit);
93  }
94  return common_limit;
95 }
96 
97 JointLimit JointLimitsContainer::getLimit(const std::string& joint_name) const
98 {
99  return container_.at(joint_name);
100 }
101 
102 std::map<std::string, JointLimit>::const_iterator JointLimitsContainer::begin() const
103 {
104  return container_.begin();
105 }
106 
107 std::map<std::string, JointLimit>::const_iterator JointLimitsContainer::end() const
108 {
109  return container_.end();
110 }
111 
112 bool JointLimitsContainer::verifyVelocityLimit(const std::string& joint_name, const double& joint_velocity) const
113 {
114  return (!(hasLimit(joint_name) && getLimit(joint_name).has_velocity_limits &&
115  fabs(joint_velocity) > getLimit(joint_name).max_velocity));
116 }
117 
118 bool JointLimitsContainer::verifyPositionLimit(const std::string& joint_name, const double& joint_position) const
119 {
120  return (!(hasLimit(joint_name) && getLimit(joint_name).has_position_limits &&
121  (joint_position < getLimit(joint_name).min_position || joint_position > getLimit(joint_name).max_position)));
122 }
123 
124 void JointLimitsContainer::updateCommonLimit(const JointLimit& joint_limit, JointLimit& common_limit)
125 {
126  // check position limits
127  if (joint_limit.has_position_limits)
128  {
129  double min_position = joint_limit.min_position;
130  double max_position = joint_limit.max_position;
131 
132  common_limit.min_position =
133  (!common_limit.has_position_limits) ? min_position : std::max(common_limit.min_position, min_position);
134  common_limit.max_position =
135  (!common_limit.has_position_limits) ? max_position : std::min(common_limit.max_position, max_position);
136  common_limit.has_position_limits = true;
137  }
138 
139  // check velocity limits
140  if (joint_limit.has_velocity_limits)
141  {
142  double max_velocity = joint_limit.max_velocity;
143  common_limit.max_velocity =
144  (!common_limit.has_velocity_limits) ? max_velocity : std::min(common_limit.max_velocity, max_velocity);
145  common_limit.has_velocity_limits = true;
146  }
147 
148  // check acceleration limits
149  if (joint_limit.has_acceleration_limits)
150  {
151  double max_acc = joint_limit.max_acceleration;
152  common_limit.max_acceleration =
153  (!common_limit.has_acceleration_limits) ? max_acc : std::min(common_limit.max_acceleration, max_acc);
154  common_limit.has_acceleration_limits = true;
155  }
156 
157  // check deceleration limits
158  if (joint_limit.has_deceleration_limits)
159  {
160  double max_dec = joint_limit.max_deceleration;
161  common_limit.max_deceleration =
162  (!common_limit.has_deceleration_limits) ? max_dec : std::max(common_limit.max_deceleration, max_dec);
163  common_limit.has_deceleration_limits = true;
164  }
165 }
166 
167 } // namespace pilz_industrial_motion_planner
JointLimit getLimit(const std::string &joint_name) const
getLimit get the limit for the given joint name
bool hasLimit(const std::string &joint_name) const
Check if there is a limit for a joint with the given name in this container.
bool verifyVelocityLimit(const std::string &joint_name, const double &joint_velocity) const
verify position limit of single joint
bool verifyPositionLimit(const std::string &joint_name, const double &joint_position) const
verify position limit of single joint
std::map< std::string, JointLimit >::const_iterator end() const
ConstIterator to the underlying data structure.
std::map< std::string, JointLimit > container_
Actual container object containing the data.
bool addLimit(const std::string &joint_name, JointLimit joint_limit)
Add a limit.
std::map< std::string, JointLimit >::const_iterator begin() const
ConstIterator to the underlying data structure.
size_t getCount() const
Get Number of limits in the container.
bool empty() const
Returns whether the container is empty.
JointLimit getCommonLimit() const
Returns joint limit fusion of all(position, velocity, acceleration, deceleration) limits for all join...
Extends joint_limits_interface::JointLimits with a deceleration parameter.
double max_deceleration
maximum deceleration MUST(!) be negative