joint_saturation_limiter.hpp

// Copyright (c) 2024, PickNik Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
 
#ifndef JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_
#define JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_
 
#include <memory>
#include <vector>
 
#include "joint_limits/joint_limiter_interface.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/duration.hpp"
 
namespace joint_limits
{
template <typename JointLimitsStateDataType>
class JointSaturationLimiter : public JointLimiterInterface<JointLimitsStateDataType>
{
public:
  JointSaturationLimiter();
 
  virtual ~JointSaturationLimiter();
 
  bool on_init() override { return true; }
 
  bool on_configure(const JointLimitsStateDataType & current_joint_states) override
  {
    prev_command_ = current_joint_states;
    return true;
  }
 
  bool on_enforce(
    const JointLimitsStateDataType & current_joint_states,
    JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) override;
 
  void reset_internals() override
  {
    std::lock_guard<std::mutex> lock(mutex_);
    prev_command_ = JointLimitsStateDataType();
  }
 
protected:
  rclcpp::Clock::SharedPtr clock_;
  JointLimitsStateDataType prev_command_;
  std::mutex mutex_;
};
 
template <typename JointLimitsStateDataType>
JointSaturationLimiter<JointLimitsStateDataType>::JointSaturationLimiter()
: JointLimiterInterface<JointLimitsStateDataType>()
{
  clock_ = std::make_shared<rclcpp::Clock>(rclcpp::Clock(RCL_ROS_TIME));
}
 
template <typename JointLimitsStateDataType>
JointSaturationLimiter<JointLimitsStateDataType>::~JointSaturationLimiter()
{
}
 
}  // namespace joint_limits
 
#endif  // JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_