control_policies.h

Go to the documentation of this file.

// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2020 FZI Forschungszentrum Informatik
// Created on behalf of Universal Robots A/S
//
// 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.
// -- END LICENSE BLOCK ------------------------------------------------

//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------

#pragma once

#include <controller_interface/multi_interface_controller.h>
#include <speed_scaling_interface/speed_scaling_interface.h>
#include <hardware_interface/robot_hw.h>
#include <hardware_interface/joint_command_interface.h>
#include <cartesian_interface/cartesian_command_interface.h>
#include <cartesian_trajectory_interpolation/cartesian_trajectory.h>
#include "cartesian_trajectory_interpolation/cartesian_trajectory_segment.h"
#include "hardware_interface/joint_state_interface.h"
#include "kdl/chainfksolver.hpp"
#include <cartesian_trajectory_interpolation/cartesian_state.h>
#include <pluginlib/class_loader.h>
#include <inverse_kinematics/ik_solver_base.h>

// KDL
#include <kdl/chain.hpp>
#include <kdl/chainfksolvervel_recursive.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl/chainiksolvervel_wdls.hpp>
#include <memory>

namespace ros_controllers_cartesian
{
template <class HWInterface>
using Controller =
    controller_interface::MultiInterfaceController<HWInterface, scaled_controllers::SpeedScalingInterface>;

template <class HWInterface, class HandleType>
class JointBasedController : public Controller<HWInterface>
{
public:
  JointBasedController() : Controller<HWInterface>(true){};  // optional speedscaling

  virtual bool init(hardware_interface::RobotHW* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override;

  CartesianState getState() const;

protected:
  std::vector<HandleType> joint_handles_;
  std::unique_ptr<KDL::ChainFkSolverVel_recursive> fk_solver_;
  KDL::Chain robot_chain_;
  std::string robot_base_;
  std::string robot_tip_;
};

template <class HWInterface>
class ControlPolicy;

template <>
class ControlPolicy<ros_controllers_cartesian::PoseCommandInterface>
  : public Controller<ros_controllers_cartesian::PoseCommandInterface>
{
public:
  ControlPolicy() : Controller<ros_controllers_cartesian::PoseCommandInterface>(true){};  // optional speedscaling

  bool init(hardware_interface::RobotHW* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override;

  void updateCommand(const CartesianState& cmd);

  CartesianState getState() const;

private:
  std::string base_;
  std::string tip_;
  ros_controllers_cartesian::PoseCommandHandle handle_;
};

template <>
class ControlPolicy<ros_controllers_cartesian::TwistCommandInterface>
  : public Controller<ros_controllers_cartesian::TwistCommandInterface>
{
public:
  ControlPolicy()
    : Controller<ros_controllers_cartesian::TwistCommandInterface>(true)  // optional speedscaling
    {};

  void updateCommand(const ros_controllers_cartesian::CartesianState& cmd);

  ros_controllers_cartesian::CartesianState getState() const;
};

template <>
class ControlPolicy<hardware_interface::PositionJointInterface>
  : public JointBasedController<hardware_interface::PositionJointInterface, hardware_interface::JointHandle>
{
public:
  using Base = JointBasedController<hardware_interface::PositionJointInterface, hardware_interface::JointHandle>;

  bool init(hardware_interface::RobotHW* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override;

  void updateCommand(const CartesianState& cmd);

private:
  std::unique_ptr<pluginlib::ClassLoader<IKSolver> > solver_loader_;
  std::unique_ptr<IKSolver> ik_solver_;
};

template <>
class ControlPolicy<hardware_interface::VelocityJointInterface>
  : public JointBasedController<hardware_interface::VelocityJointInterface, hardware_interface::JointHandle>
{
public:
  using Base = JointBasedController<hardware_interface::VelocityJointInterface, hardware_interface::JointHandle>;

  bool init(hardware_interface::RobotHW* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override;

  void updateCommand(const CartesianState& cmd);

private:
  std::unique_ptr<KDL::ChainIkSolverVel_wdls> ik_solver_;
};

template <>
class ControlPolicy<hardware_interface::JointStateInterface>
  : public JointBasedController<hardware_interface::JointStateInterface, hardware_interface::JointStateHandle>
{
public:
  using Base = JointBasedController<hardware_interface::JointStateInterface, hardware_interface::JointStateHandle>;

  bool init(hardware_interface::RobotHW* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override;

  void updateCommand(const CartesianState& cmd);

private:
  ros::Publisher pose_publisher_;
  ros::Publisher twist_publisher_;
};

}  // namespace ros_controllers_cartesian

#include <cartesian_trajectory_controller/control_policies.hpp>