qb_device_joint_limits_interface.h
Go to the documentation of this file.
1 /***
2  * Software License Agreement: BSD 3-Clause License
3  *
4  * Copyright (c) 2016-2018, qbrobotics®
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
8  * following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright notice, this list of conditions and the
11  * following disclaimer.
12  *
13  * * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
14  * following disclaimer in the documentation and/or other materials provided with the distribution.
15  *
16  * * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote
17  * products derived from this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
23  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
24  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
25  * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  */
27 
28 #ifndef QB_DEVICE_JOINT_LIMITS_INTERFACE_H
29 #define QB_DEVICE_JOINT_LIMITS_INTERFACE_H
30 
38  public:
45  : joint_handle_(joint_handle),
46  limits_(limits),
47  command_old_(std::numeric_limits<double>::quiet_NaN()) {
49  limits_->min_position = -std::numeric_limits<double>::max();
50  limits_->max_position = std::numeric_limits<double>::max();
51  }
52  }
53 
59  void enforceLimits(const ros::Duration &period) {
60  if (std::isnan(command_old_)) {
61  reset();
62  }
63 
64  double min_pos, max_pos;
66  const double delta_pos = limits_->max_velocity * period.toSec();
67  min_pos = std::max(command_old_ - delta_pos, limits_->min_position);
68  max_pos = std::min(command_old_ + delta_pos, limits_->max_position);
69  }
70  else {
71  min_pos = limits_->min_position;
72  max_pos = limits_->max_position;
73  }
74 
76  ROS_WARN_STREAM_COND(joint_handle_.getCommand() < (min_pos - 0.035) || joint_handle_.getCommand() > (max_pos + 0.035), "Limit reached for joint " << joint_handle_.getName() << " (" << joint_handle_.getCommand() << ")"); // 0.035 rad is 2 degrees
77  joint_handle_.setCommand(command);
78  command_old_ = command;
79  }
80 
84  std::string getName() const { return joint_handle_.getName(); }
85 
90 
91  private:
94  double command_old_;
95 };
96 
128 //class PositionJointSoftLimitsHandle
129 //{
130 // public:
131 // /**
132 // * Construct the saturation handle with \b dynamic joint limits.
133 // * \param joint_handle The joint handle of interest.
134 // * \param limits The \p pointer to the joint limits structure of the joint.
135 // */
136 // PositionJointSoftLimitsHandle(const hardware_interface::JointHandle &joint_handle, joint_limits_interface::JointLimits *limits, joint_limits_interface::SoftJointLimits *soft_limits)
137 // : joint_handle_(joint_handle),
138 // limits_(limits),
139 // soft_limits_(soft_limits),
140 // command_old_(std::numeric_limits<double>::quiet_NaN()) {
141 // if (!limits_->has_position_limits) {
142 // limits_->min_position = -std::numeric_limits<double>::max();
143 // limits_->max_position = std::numeric_limits<double>::max();
144 // }
145 // //TODO: check
146 // if (!limits.has_velocity_limits) {
147 // throw JointLimitsInterfaceException("Cannot enforce limits for joint '" + getName() +
148 // "'. It has no velocity limits specification.");
149 // }
150 // }
151 //
152 // /**
153 // * Enforce position and velocity limits for a joint that is subject to soft limits.
154 // * \note: Be aware of dynamic joint limits.
155 // * \param period The control period.
156 // */
157 // void enforceLimits(const ros::Duration &period) {
158 // if (std::isnan(command_old_)) {
159 // reset();
160 // }
161 //
162 // double current_position = joint_handle_.getPosition();
163 // if (limits_.has_position_limits) {
164 //
165 // }
166 // else {
167 //
168 // }
169 //
170 // const double command = joint_limits_interface::internal::saturate(joint_handle_.getCommand(), min_pos, max_pos);
171 // joint_handle_.setCommand(command);
172 // command_old_ = command;
173 // }
174 //
175 // /**
176 // * \return The joint name.
177 // */
178 // std::string getName() const { return joint_handle_.getName(); }
179 //
180 // /**
181 // * Reset the interface state.
182 // */
183 // void reset() { command_old_ = joint_handle_.getPosition(); }
184 //
185 // private:
186 // hardware_interface::JointHandle joint_handle_;
187 // joint_limits_interface::JointLimits *limits_;
188 // joint_limits_interface::JointLimits *soft_limits_;
189 // double command_old_;
190 //};
191 
198  public:
200 
203  void reset() {
205  for (ItratorType it = this->resource_map_.begin(); it != this->resource_map_.end(); ++it) {
206  it->second.reset();
207  }
208  }
210 };
211 
212 //class PositionJointSoftLimitsInterface : public joint_limits_interface::JointLimitsInterface<PositionJointSoftLimitsHandle> {
213 // public:
214 // /** \name Real-Time Safe Functions
215 // /**
216 // * Reset all managed handles.
217 // */
218 // void reset() {
219 // typedef hardware_interface::ResourceManager<PositionJointSoftLimitsHandle>::ResourceMap::iterator ItratorType;
220 // for (ItratorType it = this->resource_map_.begin(); it != this->resource_map_.end(); ++it) {
221 // it->second.reset();
222 // }
223 // }
224 // /// \}
225 //};
226 } // namespace qb_device_joint_limits_interface
227 
228 #endif // QB_DEVICE_JOINT_LIMITS_INTERFACE_H
A handle used to enforce position and velocity limits of a position-controlled joint.
void enforceLimits(const ros::Duration &period)
Enforce position and velocity limits for a joint that is not subject to soft limits.
ROSLIB_DECL std::string command(const std::string &cmd)
#define ROS_WARN_STREAM_COND(cond, args)
void setCommand(double command)
PositionJointSaturationHandle(const hardware_interface::JointHandle &joint_handle, joint_limits_interface::JointLimits *limits)
Construct the saturation handle with dynamic joint limits.
A handle used to enforce position and velocity limits of a position-controlled joint that does not ha...
T saturate(const T val, const T min_val, const T max_val)


qb_device_hardware_interface
Author(s): qbrobotics®
autogenerated on Thu Jun 6 2019 19:46:29