From f5b8708dcee0f484d540aa8206bbad06014a083e Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 23 Mar 2019 15:03:18 +0900 Subject: [PATCH] add joint_trajectory_controller from https://github.com/ros-controls/ros_controllers/pull/411 --- gundam_rx78_control/CMakeLists.txt | 29 +- .../joint_trajectory_controller.h | 266 ++++++ .../joint_trajectory_controller_impl.h | 900 ++++++++++++++++++ .../joint_trajectory_controller.xml | 12 + gundam_rx78_control/package.xml | 8 +- .../src/joint_trajectory_controller.cpp | 95 ++ 6 files changed, 1307 insertions(+), 3 deletions(-) create mode 100644 gundam_rx78_control/include/gundam_rx78_control/joint_trajectory_controller.h create mode 100644 gundam_rx78_control/include/gundam_rx78_control/joint_trajectory_controller_impl.h create mode 100644 gundam_rx78_control/joint_trajectory_controller.xml create mode 100644 gundam_rx78_control/src/joint_trajectory_controller.cpp diff --git a/gundam_rx78_control/CMakeLists.txt b/gundam_rx78_control/CMakeLists.txt index 4c2aa46..45a6338 100644 --- a/gundam_rx78_control/CMakeLists.txt +++ b/gundam_rx78_control/CMakeLists.txt @@ -1,12 +1,27 @@ cmake_minimum_required(VERSION 2.8.3) project(gundam_rx78_control) -find_package(catkin REQUIRED) +find_package(catkin REQUIRED COMPONENTS pluginlib) ################################### ## catkin specific configuration ## ################################### -catkin_package() +catkin_package( + CATKIN_DEPENDS pluginlib + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + ) + +########### +## Build ## +########### +include_directories(include ${catkin_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME} + src/joint_trajectory_controller.cpp +) + +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) ############# ## Install ## @@ -16,6 +31,16 @@ install(DIRECTORY config launch sample USE_SOURCE_PERMISSIONS ) +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + +install(FILES joint_trajectory_controller.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + ############# ## Testing ## ############# diff --git a/gundam_rx78_control/include/gundam_rx78_control/joint_trajectory_controller.h b/gundam_rx78_control/include/gundam_rx78_control/joint_trajectory_controller.h new file mode 100644 index 0000000..4e15d24 --- /dev/null +++ b/gundam_rx78_control/include/gundam_rx78_control/joint_trajectory_controller.h @@ -0,0 +1,266 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// Copyright (c) 2008, Willow Garage, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics S.L. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Adolfo Rodriguez Tsouroukdissian, Stuart Glaser + +#ifndef JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_CONTROLLER_H +#define JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_CONTROLLER_H + +// C++ standard +#include +#include +#include +#include + +// Boost +#include +#include +#include + +// ROS +#include + +// URDF +#include + +// ROS messages +#include +#include +#include +#include + +// actionlib +#include + +// realtime_tools +#include +#include +#include + +// ros_controls +#include +#include +#include +#include + +// Project +#include + +#include +#include +#include + +namespace joint_trajectory_controller +{ + +/** + * \brief Controller for executing joint-space trajectories on a group of joints. + * + * \note Non-developer documentation and usage instructions can be found in the package's ROS wiki page. + * + * \tparam SegmentImpl Trajectory segment representation to use. The type must comply with the following structure: + * \code + * class FooSegment + * { + * public: + * // Required types + * typedef double Scalar; // Scalar can be anything convertible to double + * typedef Scalar Time; + * typedef PosVelAccState State; + * + * // Default constructor + * FooSegment(); + * + * // Constructor from start and end states (boundary conditions) + * FooSegment(const Time& start_time, + * const State& start_state, + * const Time& end_time, + * const State& end_state); + * + * // Start and end states initializer (the guts of the above constructor) + * // May throw std::invalid_argument if parameters are invalid + * void init(const Time& start_time, + * const State& start_state, + * const Time& end_time, + * const State& end_state); + * + * // Sampler (realtime-safe) + * void sample(const Time& time, State& state) const; + * + * // Accesors (realtime-safe) + * Time startTime() const; + * Time endTime() const; + * unsigned int size() const; + * }; + * \endcode + * + * \tparam HardwareInterface Controller hardware interface. Currently \p hardware_interface::PositionJointInterface, + * \p hardware_interface::VelocityJointInterface, and \p hardware_interface::EffortJointInterface are supported + * out-of-the-box. + */ +template +class JointTrajectoryController : public controller_interface::Controller +{ +public: + + JointTrajectoryController(); + + /** \name Non Real-Time Safe Functions + *\{*/ + bool init(HardwareInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh); + /*\}*/ + + /** \name Real-Time Safe Functions + *\{*/ + /** \brief Holds the current position. */ + void starting(const ros::Time& time); + + /** \brief Cancels the active action goal, if any. */ + void stopping(const ros::Time& /*time*/); + + void update(const ros::Time& time, const ros::Duration& period); + /*\}*/ + +protected: + + struct TimeData + { + TimeData() : time(0.0), period(0.0), uptime(0.0) {} + + ros::Time time; ///< Time of last update cycle + ros::Duration period; ///< Period of last update cycle + ros::Time uptime; ///< Controller uptime. Set to zero at every restart. + }; + + typedef actionlib::ActionServer ActionServer; + typedef boost::shared_ptr ActionServerPtr; + typedef ActionServer::GoalHandle GoalHandle; + typedef realtime_tools::RealtimeServerGoalHandle RealtimeGoalHandle; + typedef boost::shared_ptr RealtimeGoalHandlePtr; + typedef trajectory_msgs::JointTrajectory::ConstPtr JointTrajectoryConstPtr; + typedef realtime_tools::RealtimePublisher StatePublisher; + typedef boost::scoped_ptr StatePublisherPtr; + + typedef JointTrajectorySegment Segment; + typedef std::vector TrajectoryPerJoint; + typedef std::vector Trajectory; + typedef boost::shared_ptr TrajectoryPtr; + typedef boost::shared_ptr TrajectoryPerJointPtr; + typedef realtime_tools::RealtimeBox TrajectoryBox; + typedef typename Segment::Scalar Scalar; + + typedef HardwareInterfaceAdapter HwIfaceAdapter; + typedef typename HardwareInterface::ResourceHandleType JointHandle; + + bool verbose_; ///< Hard coded verbose flag to help in debugging + std::string name_; ///< Controller name. + std::vector joints_; ///< Handles to controlled joints. + std::vector angle_wraparound_; ///< Whether controlled joints wrap around or not. + std::vector joint_names_; ///< Controlled joint names. + SegmentTolerances default_tolerances_; ///< Default trajectory segment tolerances. + HwIfaceAdapter hw_iface_adapter_; ///< Adapts desired trajectory state to HW interface. + + RealtimeGoalHandlePtr rt_active_goal_; ///< Currently active action goal, if any. + + /** + * Thread-safe container with a smart pointer to trajectory currently being followed. + * Can be either a hold trajectory or a trajectory received from a ROS message. + * + * We store the hold trajectory in a separate class member because the \p starting(time) method must be realtime-safe. + * The (single segment) hold trajectory is preallocated at initialization time and its size is kept unchanged. + */ + TrajectoryBox curr_trajectory_box_; + TrajectoryPtr hold_trajectory_ptr_; ///< Last hold trajectory values. + + typename Segment::State current_state_; ///< Preallocated workspace variable. + typename Segment::State desired_state_; ///< Preallocated workspace variable. + typename Segment::State state_error_; ///< Preallocated workspace variable. + typename Segment::State desired_joint_state_; ///< Preallocated workspace variable. + typename Segment::State state_joint_error_; ///< Preallocated workspace variable. + + realtime_tools::RealtimeBuffer time_data_; + + ros::Duration state_publisher_period_; + ros::Duration action_monitor_period_; + + typename Segment::Time stop_trajectory_duration_; ///< Duration for stop ramp. If zero, the controller stops at the actual position. + boost::dynamic_bitset<> successful_joint_traj_; + bool allow_partial_joints_goal_; + + // ROS API + ros::NodeHandle controller_nh_; + ros::Subscriber trajectory_command_sub_; + ActionServerPtr action_server_; + ros::ServiceServer query_state_service_; + StatePublisherPtr state_publisher_; + + ros::Timer goal_handle_timer_; + ros::Time last_state_publish_time_; + + // Mimic Joints + std::vector mimic_joints_; ///< Handles to mimic joints. + std::vector mimic_joint_names_; ///< Mimic joint names. + std::vector mimic_urdf_joints_; ///< URDF joint of mimic joints + std::vector mimic_joint_ids_; ///< index of target joint in joints_ vector + + typename Segment::State mimic_current_state_; ///< Preallocated workspace variable. + typename Segment::State mimic_desired_state_; ///< Preallocated workspace variable. + typename Segment::State mimic_state_error_; ///< Preallocated workspace variable. + HwIfaceAdapter mimic_hw_iface_adapter_; ///< Adapts desired trajectory state to HW interface. + + virtual bool updateTrajectoryCommand(const JointTrajectoryConstPtr& msg, RealtimeGoalHandlePtr gh, std::string* error_string = 0); + virtual void trajectoryCommandCB(const JointTrajectoryConstPtr& msg); + virtual void goalCB(GoalHandle gh); + virtual void cancelCB(GoalHandle gh); + virtual void preemptActiveGoal(); + virtual bool queryStateService(control_msgs::QueryTrajectoryState::Request& req, + control_msgs::QueryTrajectoryState::Response& resp); + + /** + * \brief Publish current controller state at a throttled frequency. + * \note This method is realtime-safe and is meant to be called from \ref update, as it shares data with it without + * any locking. + */ + void publishState(const ros::Time& time); + + /** + * \brief Hold the current position. + * + * Substitutes the current trajectory with a single-segment one going from the current position and velocity to + * zero velocity. + * \see parameter stop_trajectory_duration + * \note This method is realtime-safe. + */ + void setHoldPosition(const ros::Time& time, RealtimeGoalHandlePtr gh=RealtimeGoalHandlePtr()); + +}; + +} // namespace + +#include "gundam_rx78_control/joint_trajectory_controller_impl.h" +#endif // header guard diff --git a/gundam_rx78_control/include/gundam_rx78_control/joint_trajectory_controller_impl.h b/gundam_rx78_control/include/gundam_rx78_control/joint_trajectory_controller_impl.h new file mode 100644 index 0000000..bde2c69 --- /dev/null +++ b/gundam_rx78_control/include/gundam_rx78_control/joint_trajectory_controller_impl.h @@ -0,0 +1,900 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// Copyright (c) 2008, Willow Garage, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics S.L. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Adolfo Rodriguez Tsouroukdissian, Stuart Glaser + +#ifndef JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_CONTROLLER_IMP_H +#define JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_CONTROLLER_IMP_H + + +namespace joint_trajectory_controller +{ + +namespace internal +{ + +template +inline boost::shared_ptr share_member(boost::shared_ptr enclosure, Member &member) +{ + actionlib::EnclosureDeleter d(enclosure); + boost::shared_ptr p(&member, d); + return p; +} + +std::vector getStrings(const ros::NodeHandle& nh, const std::string& param_name) +{ + using namespace XmlRpc; + XmlRpcValue xml_array; + if (!nh.getParam(param_name, xml_array)) + { + ROS_ERROR_STREAM("Could not find '" << param_name << "' parameter (namespace: " << nh.getNamespace() << ")."); + return std::vector(); + } + if (xml_array.getType() != XmlRpcValue::TypeArray) + { + ROS_ERROR_STREAM("The '" << param_name << "' parameter is not an array (namespace: " << + nh.getNamespace() << ")."); + return std::vector(); + } + + std::vector out; + for (int i = 0; i < xml_array.size(); ++i) + { + if (xml_array[i].getType() != XmlRpcValue::TypeString) + { + ROS_ERROR_STREAM("The '" << param_name << "' parameter contains a non-string element (namespace: " << + nh.getNamespace() << ")."); + return std::vector(); + } + out.push_back(static_cast(xml_array[i])); + } + return out; +} + +urdf::ModelSharedPtr getUrdf(const ros::NodeHandle& nh, const std::string& param_name) +{ + urdf::ModelSharedPtr urdf(new urdf::Model); + + std::string urdf_str; + // Check for robot_description in proper namespace + if (nh.getParam(param_name, urdf_str)) + { + if (!urdf->initString(urdf_str)) + { + ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter (namespace: " << + nh.getNamespace() << ")."); + return urdf::ModelSharedPtr(); + } + } + // Check for robot_description in root + else if (!urdf->initParam("robot_description")) + { + ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter"); + return urdf::ModelSharedPtr(); + } + return urdf; +} + +std::vector getUrdfJoints(const urdf::Model& urdf, const std::vector& joint_names) +{ + std::vector out; + for (unsigned int i = 0; i < joint_names.size(); ++i) + { + urdf::JointConstSharedPtr urdf_joint = urdf.getJoint(joint_names[i]); + if (urdf_joint) + { + out.push_back(urdf_joint); + } + else + { + ROS_ERROR_STREAM("Could not find joint '" << joint_names[i] << "' in URDF model."); + return std::vector(); + } + } + return out; +} + +std::string getLeafNamespace(const ros::NodeHandle& nh) +{ + const std::string complete_ns = nh.getNamespace(); + std::size_t id = complete_ns.find_last_of("/"); + return complete_ns.substr(id + 1); +} + +} // namespace + +template +inline void JointTrajectoryController:: +starting(const ros::Time& time) +{ + // Update time data + TimeData time_data; + time_data.time = time; + time_data.uptime = ros::Time(0.0); + time_data_.initRT(time_data); + + // Initialize the desired_state with the current state on startup + for (unsigned int i = 0; i < joints_.size(); ++i) + { + desired_state_.position[i] = joints_[i].getPosition(); + desired_state_.velocity[i] = joints_[i].getVelocity(); + } + + // Hold current position + setHoldPosition(time_data.uptime); + + // Initialize last state update time + last_state_publish_time_ = time_data.uptime; + + // Hardware interface adapter + hw_iface_adapter_.starting(time_data.uptime); +} + +template +inline void JointTrajectoryController:: +stopping(const ros::Time& /*time*/) +{ + preemptActiveGoal(); +} + +template +inline void JointTrajectoryController:: +trajectoryCommandCB(const JointTrajectoryConstPtr& msg) +{ + const bool update_ok = updateTrajectoryCommand(msg, RealtimeGoalHandlePtr()); + if (update_ok) {preemptActiveGoal();} +} + +template +inline void JointTrajectoryController:: +preemptActiveGoal() +{ + RealtimeGoalHandlePtr current_active_goal(rt_active_goal_); + + // Cancels the currently active goal + if (current_active_goal) + { + // Marks the current goal as canceled + rt_active_goal_.reset(); + current_active_goal->gh_.setCanceled(); + } +} + +template +JointTrajectoryController:: +JointTrajectoryController() + : verbose_(false), // Set to true during debugging + hold_trajectory_ptr_(new Trajectory) +{ + // The verbose parameter is for advanced use as it breaks real-time safety + // by enabling ROS logging services + if (verbose_) + { + ROS_WARN_STREAM( + "The joint_trajectory_controller verbose flag is enabled. " + << "This flag breaks real-time safety and should only be " + << "used for debugging"); + } +} + +template +bool JointTrajectoryController::init(HardwareInterface* hw, + ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) +{ + using namespace internal; + + // Cache controller node handle + controller_nh_ = controller_nh; + + // Controller name + name_ = getLeafNamespace(controller_nh_); + + // State publish rate + double state_publish_rate = 50.0; + controller_nh_.getParam("state_publish_rate", state_publish_rate); + ROS_DEBUG_STREAM_NAMED(name_, "Controller state will be published at " << state_publish_rate << "Hz."); + state_publisher_period_ = ros::Duration(1.0 / state_publish_rate); + + // Action status checking update rate + double action_monitor_rate = 20.0; + controller_nh_.getParam("action_monitor_rate", action_monitor_rate); + action_monitor_period_ = ros::Duration(1.0 / action_monitor_rate); + ROS_DEBUG_STREAM_NAMED(name_, "Action status changes will be monitored at " << action_monitor_rate << "Hz."); + + // Stop trajectory duration + stop_trajectory_duration_ = 0.0; + if (!controller_nh_.getParam("stop_trajectory_duration", stop_trajectory_duration_)) + { + // TODO: Remove this check/warning in Indigo + if (controller_nh_.getParam("hold_trajectory_duration", stop_trajectory_duration_)) + { + ROS_WARN("The 'hold_trajectory_duration' has been deprecated in favor of the 'stop_trajectory_duration' parameter. Please update your controller configuration."); + } + } + ROS_DEBUG_STREAM_NAMED(name_, "Stop trajectory has a duration of " << stop_trajectory_duration_ << "s."); + + // Checking if partial trajectories are allowed + controller_nh_.param("allow_partial_joints_goal", allow_partial_joints_goal_, false); + if (allow_partial_joints_goal_) + { + ROS_DEBUG_NAMED(name_, "Goals with partial set of joints are allowed"); + } + + // List of controlled joints + joint_names_ = getStrings(controller_nh_, "joints"); + if (joint_names_.empty()) {return false;} + const unsigned int n_joints = joint_names_.size(); + + // URDF joints + urdf::ModelSharedPtr urdf = getUrdf(root_nh, "robot_description"); + if (!urdf) {return false;} + + std::vector urdf_joints = getUrdfJoints(*urdf, joint_names_); + if (urdf_joints.empty()) {return false;} + assert(n_joints == urdf_joints.size()); + + // Initialize members + joints_.resize(n_joints); + angle_wraparound_.resize(n_joints); + for (unsigned int i = 0; i < n_joints; ++i) + { + // Joint handle + try {joints_[i] = hw->getHandle(joint_names_[i]);} + catch (...) + { + ROS_ERROR_STREAM_NAMED(name_, "Could not find joint '" << joint_names_[i] << "' in '" << + this->getHardwareInterfaceType() << "'."); + return false; + } + + // Whether a joint is continuous (ie. has angle wraparound) + angle_wraparound_[i] = urdf_joints[i]->type == urdf::Joint::CONTINUOUS; + const std::string not_if = angle_wraparound_[i] ? "" : "non-"; + + ROS_DEBUG_STREAM_NAMED(name_, "Found " << not_if << "continuous joint '" << joint_names_[i] << "' in '" << + this->getHardwareInterfaceType() << "'."); + + ROS_INFO_STREAM("Loaidng joint " << joint_names_[i] << " (" << i << ")"); + } + + assert(joints_.size() == angle_wraparound_.size()); + ROS_DEBUG_STREAM_NAMED(name_, "Initialized controller '" << name_ << "' with:" << + "\n- Number of joints: " << joints_.size() << + "\n- Hardware interface type: '" << this->getHardwareInterfaceType() << "'" << + "\n- Trajectory segment type: '" << hardware_interface::internal::demangledTypeName() << "'"); + + // Default tolerances + ros::NodeHandle tol_nh(controller_nh_, "constraints"); + default_tolerances_ = getSegmentTolerances(tol_nh, joint_names_); + + // Hardware interface adapter + hw_iface_adapter_.init(joints_, controller_nh_); + + // ROS API: Subscribed topics + trajectory_command_sub_ = controller_nh_.subscribe("command", 1, &JointTrajectoryController::trajectoryCommandCB, this); + + // ROS API: Published topics + state_publisher_.reset(new StatePublisher(controller_nh_, "state", 1)); + + // ROS API: Action interface + action_server_.reset(new ActionServer(controller_nh_, "follow_joint_trajectory", + boost::bind(&JointTrajectoryController::goalCB, this, _1), + boost::bind(&JointTrajectoryController::cancelCB, this, _1), + false)); + action_server_->start(); + + // ROS API: Provided services + query_state_service_ = controller_nh_.advertiseService("query_state", + &JointTrajectoryController::queryStateService, + this); + + // Preeallocate resources + current_state_ = typename Segment::State(n_joints); + desired_state_ = typename Segment::State(n_joints); + state_error_ = typename Segment::State(n_joints); + desired_joint_state_ = typename Segment::State(1); + state_joint_error_ = typename Segment::State(1); + + successful_joint_traj_ = boost::dynamic_bitset<>(joints_.size()); + + // Initialize trajectory with all joints + typename Segment::State current_joint_state_ = typename Segment::State(1); + for (unsigned int i = 0; i < n_joints; ++i) + { + current_joint_state_.position[0]= current_state_.position[i]; + current_joint_state_.velocity[0]= current_state_.velocity[i]; + Segment hold_segment(0.0, current_joint_state_, 0.0, current_joint_state_); + + TrajectoryPerJoint joint_segment; + joint_segment.resize(1, hold_segment); + hold_trajectory_ptr_->push_back(joint_segment); + } + + { + state_publisher_->lock(); + state_publisher_->msg_.joint_names = joint_names_; + state_publisher_->msg_.desired.positions.resize(n_joints); + state_publisher_->msg_.desired.velocities.resize(n_joints); + state_publisher_->msg_.desired.accelerations.resize(n_joints); + state_publisher_->msg_.actual.positions.resize(n_joints); + state_publisher_->msg_.actual.velocities.resize(n_joints); + state_publisher_->msg_.error.positions.resize(n_joints); + state_publisher_->msg_.error.velocities.resize(n_joints); + state_publisher_->unlock(); + } + + // + // List of mimic joints + // + if (controller_nh_.hasParam("mimic_joints")) + { + mimic_joint_names_ = getStrings(controller_nh_, "mimic_joints"); + const unsigned int n_mimic_joints = mimic_joint_names_.size(); + mimic_joints_.resize(n_mimic_joints); + mimic_joint_ids_.resize(n_mimic_joints); + mimic_urdf_joints_.resize(n_mimic_joints); + for (unsigned int i = 0; i < n_mimic_joints; ++i) + { + // Mimic Joint handle + try { + mimic_joints_[i] = hw->hardware_interface::ResourceManager::getHandle(mimic_joint_names_[i]); + } + catch (...) + { + ROS_ERROR_STREAM_NAMED(name_, "Could not find joint '" << mimic_joint_names_[i] << "' in '" << + this->getHardwareInterfaceType() << "'."); + return false; + } + mimic_urdf_joints_[i] = urdf->getJoint(mimic_joint_names_[i]); + if (!(mimic_urdf_joints_[i] && mimic_urdf_joints_[i]->mimic)) + { + ROS_ERROR_STREAM_NAMED(name_, "Could not find joint '" << mimic_joint_names_[i] << "' in URDF"); + } + mimic_joint_ids_[i] = std::distance(joint_names_.begin(), std::find(joint_names_.begin(), joint_names_.end(), mimic_urdf_joints_[i]->mimic->joint_name)); + if (mimic_joint_ids_[i] == joint_names_.size()) + { + ROS_ERROR_STREAM_NAMED(name_, "Could not find joint '" << mimic_urdf_joints_[i]->mimic->joint_name << "' in joint_names"); + } + ROS_INFO_STREAM("Loaidng mimic_joint " << mimic_joint_names_[i] << " = " << joint_names_[mimic_joint_ids_[i]] << " (" << mimic_joint_ids_[i] << ") * " << mimic_urdf_joints_[i]->mimic->multiplier << " + " << mimic_urdf_joints_[i]->mimic->offset); + } + // Hardware interface adapter + mimic_hw_iface_adapter_.init(mimic_joints_, controller_nh_); + + mimic_current_state_ = typename Segment::State(n_mimic_joints); + mimic_desired_state_ = typename Segment::State(n_mimic_joints); + mimic_state_error_ = typename Segment::State(n_mimic_joints); + + // Initialize trajectory with all joints + typename Segment::State mimic_current_joint_state_ = typename Segment::State(1); + for (unsigned int i = 0; i < n_mimic_joints; ++i) + { + mimic_current_joint_state_.position[0]= mimic_current_state_.position[i]; + mimic_current_joint_state_.velocity[0]= mimic_current_state_.velocity[i]; + } + } + + return true; +} + +template +void JointTrajectoryController:: +update(const ros::Time& time, const ros::Duration& period) +{ + // Get currently followed trajectory + TrajectoryPtr curr_traj_ptr; + curr_trajectory_box_.get(curr_traj_ptr); + Trajectory& curr_traj = *curr_traj_ptr; + + // Update time data + TimeData time_data; + time_data.time = time; // Cache current time + time_data.period = period; // Cache current control period + time_data.uptime = time_data_.readFromRT()->uptime + period; // Update controller uptime + time_data_.writeFromNonRT(time_data); // TODO: Grrr, we need a lock-free data structure here! + + // NOTE: It is very important to execute the two above code blocks in the specified sequence: first get current + // trajectory, then update time data. Hopefully the following paragraph sheds a bit of light on the rationale. + // The non-rt thread responsible for processing new commands enqueues trajectories that can start at the _next_ + // control cycle (eg. zero start time) or later (eg. when we explicitly request a start time in the future). + // If we reverse the order of the two blocks above, and update the time data first; it's possible that by the time we + // fetch the currently followed trajectory, it has been updated by the non-rt thread with something that starts in the + // next control cycle, leaving the current cycle without a valid trajectory. + + // Update current state and state error + for (unsigned int i = 0; i < joints_.size(); ++i) + { + current_state_.position[i] = joints_[i].getPosition(); + current_state_.velocity[i] = joints_[i].getVelocity(); + // There's no acceleration data available in a joint handle + + typename TrajectoryPerJoint::const_iterator segment_it = sample(curr_traj[i], time_data.uptime.toSec(), desired_joint_state_); + if (curr_traj[i].end() == segment_it) + { + // Non-realtime safe, but should never happen under normal operation + ROS_ERROR_NAMED(name_, + "Unexpected error: No trajectory defined at current time. Please contact the package maintainer."); + return; + } + desired_state_.position[i] = desired_joint_state_.position[0]; + desired_state_.velocity[i] = desired_joint_state_.velocity[0]; + desired_state_.acceleration[i] = desired_joint_state_.acceleration[0]; ; + + state_joint_error_.position[0] = angles::shortest_angular_distance(current_state_.position[i],desired_joint_state_.position[0]); + state_joint_error_.velocity[0] = desired_joint_state_.velocity[0] - current_state_.velocity[i]; + state_joint_error_.acceleration[0] = 0.0; + + state_error_.position[i] = angles::shortest_angular_distance(current_state_.position[i],desired_joint_state_.position[0]); + state_error_.velocity[i] = desired_joint_state_.velocity[0] - current_state_.velocity[i]; + state_error_.acceleration[i] = 0.0; + + //Check tolerances + const RealtimeGoalHandlePtr rt_segment_goal = segment_it->getGoalHandle(); + if (rt_segment_goal && rt_segment_goal == rt_active_goal_) + { + // Check tolerances + if (time_data.uptime.toSec() < segment_it->endTime()) + { + // Currently executing a segment: check path tolerances + const SegmentTolerancesPerJoint& joint_tolerances = segment_it->getTolerances(); + if (!checkStateTolerancePerJoint(state_joint_error_, joint_tolerances.state_tolerance)) + { + if (verbose_) + { + ROS_ERROR_STREAM_NAMED(name_,"Path tolerances failed for joint: " << joint_names_[i]); + checkStateTolerancePerJoint(state_joint_error_, joint_tolerances.state_tolerance, true); + } + rt_segment_goal->preallocated_result_->error_code = + control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED; + rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_); + rt_active_goal_.reset(); + successful_joint_traj_.reset(); + } + } + else if (segment_it == --curr_traj[i].end()) + { + if (verbose_) + ROS_DEBUG_STREAM_THROTTLE_NAMED(1,name_,"Finished executing last segment, checking goal tolerances"); + + // Controller uptime + const ros::Time uptime = time_data_.readFromRT()->uptime; + + // Checks that we have ended inside the goal tolerances + const SegmentTolerancesPerJoint& tolerances = segment_it->getTolerances(); + const bool inside_goal_tolerances = checkStateTolerancePerJoint(state_joint_error_, tolerances.goal_state_tolerance); + + if (inside_goal_tolerances) + { + successful_joint_traj_[i] = 1; + } + else if (uptime.toSec() < segment_it->endTime() + tolerances.goal_time_tolerance) + { + // Still have some time left to meet the goal state tolerances + } + else + { + if (verbose_) + { + ROS_ERROR_STREAM_NAMED(name_,"Goal tolerances failed for joint: "<< joint_names_[i]); + // Check the tolerances one more time to output the errors that occurs + checkStateTolerancePerJoint(state_joint_error_, tolerances.goal_state_tolerance, true); + } + + rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED; + rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_); + rt_active_goal_.reset(); + successful_joint_traj_.reset(); + } + } + } + } + + //If there is an active goal and all segments finished successfully then set goal as succeeded + RealtimeGoalHandlePtr current_active_goal(rt_active_goal_); + if (current_active_goal and successful_joint_traj_.count() == joints_.size()) + { + current_active_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL; + current_active_goal->setSucceeded(current_active_goal->preallocated_result_); + current_active_goal.reset(); // do not publish feedback + rt_active_goal_.reset(); + successful_joint_traj_.reset(); + } + + // Hardware interface adapter: Generate and send commands + hw_iface_adapter_.updateCommand(time_data.uptime, time_data.period, + desired_state_, state_error_); + + // Set action feedback + if (current_active_goal) + { + current_active_goal->preallocated_feedback_->header.stamp = time_data_.readFromRT()->time; + current_active_goal->preallocated_feedback_->desired.positions = desired_state_.position; + current_active_goal->preallocated_feedback_->desired.velocities = desired_state_.velocity; + current_active_goal->preallocated_feedback_->desired.accelerations = desired_state_.acceleration; + current_active_goal->preallocated_feedback_->actual.positions = current_state_.position; + current_active_goal->preallocated_feedback_->actual.velocities = current_state_.velocity; + current_active_goal->preallocated_feedback_->error.positions = state_error_.position; + current_active_goal->preallocated_feedback_->error.velocities = state_error_.velocity; + current_active_goal->setFeedback( current_active_goal->preallocated_feedback_ ); + } + + // Publish state + publishState(time_data.uptime); + + // Mimic Joints + // Update current state and state error for mimic_joint + for (unsigned int i = 0; i < mimic_joints_.size(); ++i) + { + mimic_current_state_.position[i] = mimic_joints_[i].getPosition(); + mimic_current_state_.velocity[i] = mimic_joints_[i].getVelocity(); + // There's no acceleration data available in a joint handle + + unsigned int mimic_joint_id = mimic_joint_ids_[i]; + mimic_desired_state_.position[i] = desired_state_.position[mimic_joint_id] * mimic_urdf_joints_[i]->mimic->multiplier + mimic_urdf_joints_[i]->mimic->offset; + mimic_desired_state_.velocity[i] = desired_state_.velocity[mimic_joint_id]; + mimic_desired_state_.acceleration[i] = desired_state_.acceleration[mimic_joint_id]; + + mimic_state_error_.position[i] = angles::shortest_angular_distance(mimic_current_state_.position[i],mimic_desired_state_.position[i]); + mimic_state_error_.velocity[i] = mimic_desired_state_.velocity[i] - mimic_current_state_.velocity[i]; + mimic_state_error_.acceleration[i] = 0.0; + } + + // Send commands to mimic joints_ + if (mimic_joints_.size() > 0) + { + mimic_hw_iface_adapter_.updateCommand(time_data.uptime, time_data.period, + mimic_desired_state_, mimic_state_error_); + } + +} + +template +bool JointTrajectoryController:: +updateTrajectoryCommand(const JointTrajectoryConstPtr& msg, RealtimeGoalHandlePtr gh, std::string* error_string) +{ + typedef InitJointTrajectoryOptions Options; + Options options; + options.error_string = error_string; + std::string error_string_tmp; + + // Preconditions + if (!this->isRunning()) + { + error_string_tmp = "Can't accept new commands. Controller is not running."; + ROS_ERROR_STREAM_NAMED(name_, error_string_tmp); + options.setErrorString(error_string_tmp); + return false; + } + + if (!msg) + { + error_string_tmp = "Received null-pointer trajectory message, skipping."; + ROS_WARN_STREAM_NAMED(name_, error_string_tmp); + options.setErrorString(error_string_tmp); + return false; + } + + // Time data + TimeData* time_data = time_data_.readFromRT(); // TODO: Grrr, we need a lock-free data structure here! + + // Time of the next update + const ros::Time next_update_time = time_data->time + time_data->period; + + // Uptime of the next update + ros::Time next_update_uptime = time_data->uptime + time_data->period; + + // Hold current position if trajectory is empty + if (msg->points.empty()) + { + setHoldPosition(time_data->uptime, gh); + ROS_DEBUG_NAMED(name_, "Empty trajectory command, stopping."); + return true; + } + + // Trajectory initialization options + TrajectoryPtr curr_traj_ptr; + curr_trajectory_box_.get(curr_traj_ptr); + + options.other_time_base = &next_update_uptime; + options.current_trajectory = curr_traj_ptr.get(); + options.joint_names = &joint_names_; + options.angle_wraparound = &angle_wraparound_; + options.rt_goal_handle = gh; + options.default_tolerances = &default_tolerances_; + options.allow_partial_joints_goal = allow_partial_joints_goal_; + + // Update currently executing trajectory + try + { + TrajectoryPtr traj_ptr(new Trajectory); + *traj_ptr = initJointTrajectory(*msg, next_update_time, options); + if (!traj_ptr->empty()) + { + curr_trajectory_box_.set(traj_ptr); + } + else + { + return false; + } + } + catch(const std::invalid_argument& ex) + { + ROS_ERROR_STREAM_NAMED(name_, ex.what()); + options.setErrorString(ex.what()); + return false; + } + catch(...) + { + error_string_tmp = "Unexpected exception caught when initializing trajectory from ROS message data."; + ROS_ERROR_STREAM_NAMED(name_, error_string_tmp); + options.setErrorString(error_string_tmp); + return false; + } + + return true; +} + +template +void JointTrajectoryController:: +goalCB(GoalHandle gh) +{ + ROS_DEBUG_STREAM_NAMED(name_,"Received new action goal"); + + // Precondition: Running controller + if (!this->isRunning()) + { + ROS_ERROR_NAMED(name_, "Can't accept new action goals. Controller is not running."); + control_msgs::FollowJointTrajectoryResult result; + result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL; // TODO: Add better error status to msg? + gh.setRejected(result); + return; + } + + // If partial joints goals are not allowed, goal should specify all controller joints + if (!allow_partial_joints_goal_) + { + if (gh.getGoal()->trajectory.joint_names.size() != joint_names_.size()) + { + ROS_ERROR_NAMED(name_, "Joints on incoming goal don't match the controller joints."); + control_msgs::FollowJointTrajectoryResult result; + result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS; + gh.setRejected(result); + return; + } + } + + // Goal should specify valid controller joints (they can be ordered differently). Reject if this is not the case + using internal::mapping; + std::vector mapping_vector = mapping(gh.getGoal()->trajectory.joint_names, joint_names_); + + if (mapping_vector.empty()) + { + ROS_ERROR_NAMED(name_, "Joints on incoming goal don't match the controller joints."); + control_msgs::FollowJointTrajectoryResult result; + result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS; + gh.setRejected(result); + return; + } + + // Try to update new trajectory + RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh)); + std::string error_string; + const bool update_ok = updateTrajectoryCommand(internal::share_member(gh.getGoal(), gh.getGoal()->trajectory), + rt_goal, + &error_string); + rt_goal->preallocated_feedback_->joint_names = joint_names_; + + if (update_ok) + { + // Accept new goal + preemptActiveGoal(); + gh.setAccepted(); + rt_active_goal_ = rt_goal; + + // Setup goal status checking timer + goal_handle_timer_ = controller_nh_.createTimer(action_monitor_period_, + &RealtimeGoalHandle::runNonRealtime, + rt_goal); + goal_handle_timer_.start(); + } + else + { + // Reject invalid goal + control_msgs::FollowJointTrajectoryResult result; + result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL; + result.error_string = error_string; + gh.setRejected(result); + } +} + +template +void JointTrajectoryController:: +cancelCB(GoalHandle gh) +{ + RealtimeGoalHandlePtr current_active_goal(rt_active_goal_); + + // Check that cancel request refers to currently active goal (if any) + if (current_active_goal && current_active_goal->gh_ == gh) + { + // Reset current goal + rt_active_goal_.reset(); + + // Controller uptime + const ros::Time uptime = time_data_.readFromRT()->uptime; + + // Enter hold current position mode + setHoldPosition(uptime); + ROS_DEBUG_NAMED(name_, "Canceling active action goal because cancel callback recieved from actionlib."); + + // Mark the current goal as canceled + current_active_goal->gh_.setCanceled(); + } +} + +template +bool JointTrajectoryController:: +queryStateService(control_msgs::QueryTrajectoryState::Request& req, + control_msgs::QueryTrajectoryState::Response& resp) +{ + // Preconditions + if (!this->isRunning()) + { + ROS_ERROR_NAMED(name_, "Can't sample trajectory. Controller is not running."); + return false; + } + + // Convert request time to internal monotonic representation + TimeData* time_data = time_data_.readFromRT(); + const ros::Duration time_offset = req.time - time_data->time; + const ros::Time sample_time = time_data->uptime + time_offset; + + // Sample trajectory at requested time + TrajectoryPtr curr_traj_ptr; + curr_trajectory_box_.get(curr_traj_ptr); + Trajectory& curr_traj = *curr_traj_ptr; + + typename Segment::State response_point = typename Segment::State(joint_names_.size()); + + for (unsigned int i = 0; i < joints_.size(); ++i) + { + typename Segment::State state; + typename TrajectoryPerJoint::const_iterator segment_it = sample(curr_traj[i], sample_time.toSec(), state); + if (curr_traj[i].end() == segment_it) + { + ROS_ERROR_STREAM_NAMED(name_, "Requested sample time precedes trajectory start time."); + return false; + } + + response_point.position[i] = state.position[0]; + response_point.velocity[i] = state.velocity[0]; + response_point.acceleration[i] = state.acceleration[0]; + } + + // Populate response + resp.name = joint_names_; + resp.position = response_point.position; + resp.velocity = response_point.velocity; + resp.acceleration = response_point.acceleration; + + return true; +} + +template +void JointTrajectoryController:: +publishState(const ros::Time& time) +{ + // Check if it's time to publish + if (!state_publisher_period_.isZero() && last_state_publish_time_ + state_publisher_period_ < time) + { + if (state_publisher_ && state_publisher_->trylock()) + { + last_state_publish_time_ += state_publisher_period_; + + state_publisher_->msg_.header.stamp = time_data_.readFromRT()->time; + state_publisher_->msg_.desired.positions = desired_state_.position; + state_publisher_->msg_.desired.velocities = desired_state_.velocity; + state_publisher_->msg_.desired.accelerations = desired_state_.acceleration; + state_publisher_->msg_.actual.positions = current_state_.position; + state_publisher_->msg_.actual.velocities = current_state_.velocity; + state_publisher_->msg_.error.positions = state_error_.position; + state_publisher_->msg_.error.velocities = state_error_.velocity; + + state_publisher_->unlockAndPublish(); + } + } +} + +template +void JointTrajectoryController:: +setHoldPosition(const ros::Time& time, RealtimeGoalHandlePtr gh) +{ + assert(joint_names_.size() == hold_trajectory_ptr_->size()); + + typename Segment::State hold_start_state_ = typename Segment::State(1); + typename Segment::State hold_end_state_ = typename Segment::State(1); + const unsigned int n_joints = joints_.size(); + const typename Segment::Time start_time = time.toSec(); + + if(stop_trajectory_duration_ == 0.0) + { + // stop at current actual position + for (unsigned int i = 0; i < n_joints; ++i) + { + hold_start_state_.position[0] = joints_[i].getPosition(); + hold_start_state_.velocity[0] = 0.0; + hold_start_state_.acceleration[0] = 0.0; + (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_, + start_time, hold_start_state_); + // Set goal handle for the segment + (*hold_trajectory_ptr_)[i].front().setGoalHandle(gh); + } + } + else + { + // Settle position in a fixed time. We do the following: + // - Create segment that goes from current (pos,vel) to (pos,-vel) in 2x the desired stop time + // - Assuming segment symmetry, sample segment at its midpoint (desired stop time). It should have zero velocity + // - Create segment that goes from current state to above zero velocity state, in the desired time + // NOTE: The symmetry assumption from the second point above might not hold for all possible segment types + + const typename Segment::Time end_time = time.toSec() + stop_trajectory_duration_; + const typename Segment::Time end_time_2x = time.toSec() + 2.0 * stop_trajectory_duration_; + + // Create segment that goes from current (pos,vel) to (pos,-vel) + for (unsigned int i = 0; i < n_joints; ++i) + { + // If there is a time delay in the system it is better to calculate the hold trajectory starting from the + // desired position. Otherwise there would be a jerk in the motion. + hold_start_state_.position[0] = desired_state_.position[i]; + hold_start_state_.velocity[0] = desired_state_.velocity[i]; + hold_start_state_.acceleration[0] = 0.0; + + hold_end_state_.position[0] = desired_state_.position[i]; + hold_end_state_.velocity[0] = -desired_state_.velocity[i]; + hold_end_state_.acceleration[0] = 0.0; + + (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_, + end_time_2x, hold_end_state_); + + // Sample segment at its midpoint, that should have zero velocity + (*hold_trajectory_ptr_)[i].front().sample(end_time, hold_end_state_); + + // Now create segment that goes from current state to one with zero end velocity + (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_, + end_time, hold_end_state_); + + // Set goal handle for the segment + (*hold_trajectory_ptr_)[i].front().setGoalHandle(gh); + } + } + curr_trajectory_box_.set(hold_trajectory_ptr_); +} + +} // namespace + +#endif // header guard diff --git a/gundam_rx78_control/joint_trajectory_controller.xml b/gundam_rx78_control/joint_trajectory_controller.xml new file mode 100644 index 0000000..3582531 --- /dev/null +++ b/gundam_rx78_control/joint_trajectory_controller.xml @@ -0,0 +1,12 @@ + + + + + The JointTrajectoryController executes joint-space trajectories on a set of joints. + This variant represents trajectory segments as quintic splines and sends commands to an effort interface. + + + + diff --git a/gundam_rx78_control/package.xml b/gundam_rx78_control/package.xml index 8c342f9..9897ce0 100644 --- a/gundam_rx78_control/package.xml +++ b/gundam_rx78_control/package.xml @@ -19,8 +19,14 @@ ros_control ros_controllers + + controller_interface + pluginlib + roslaunch - + + + diff --git a/gundam_rx78_control/src/joint_trajectory_controller.cpp b/gundam_rx78_control/src/joint_trajectory_controller.cpp new file mode 100644 index 0000000..bc5dcf7 --- /dev/null +++ b/gundam_rx78_control/src/joint_trajectory_controller.cpp @@ -0,0 +1,95 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// Copyright (c) 2008, Willow Garage, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics S.L. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +// Pluginlib +#include + +// Project +#include +#include "gundam_rx78_control/joint_trajectory_controller.h" + +namespace position_controllers +{ + /** + * \brief Joint trajectory controller that represents trajectory segments as quintic splines and sends + * commands to a \b position interface. + */ + typedef joint_trajectory_controller::JointTrajectoryController, + hardware_interface::PositionJointInterface> + JointTrajectoryController; +} + +namespace velocity_controllers +{ + /** + * \brief Joint trajectory controller that represents trajectory segments as quintic splines and sends + * commands to a \b velocity interface. + */ + typedef joint_trajectory_controller::JointTrajectoryController, + hardware_interface::VelocityJointInterface> + JointTrajectoryController; +} + +namespace effort_controllers +{ + /** + * \brief Joint trajectory controller that represents trajectory segments as quintic splines and sends + * commands to an \b effort interface. + */ + typedef joint_trajectory_controller::JointTrajectoryController, + hardware_interface::EffortJointInterface> + JointTrajectoryController; +} + +namespace pos_vel_controllers +{ + /** + * \brief Joint trajectory controller that represents trajectory segments as quintic splines and sends + * commands to an \b pos_vel interface. + */ + typedef joint_trajectory_controller::JointTrajectoryController, + hardware_interface::PosVelJointInterface> + JointTrajectoryController; +} + +namespace pos_vel_acc_controllers +{ + /** + * \brief Joint trajectory controller that represents trajectory segments as quintic splines and sends + * commands to a \b pos_vel_acc interface. + */ + typedef joint_trajectory_controller::JointTrajectoryController, + hardware_interface::PosVelAccJointInterface> + JointTrajectoryController; +} + +PLUGINLIB_EXPORT_CLASS(position_controllers::JointTrajectoryController, controller_interface::ControllerBase) +PLUGINLIB_EXPORT_CLASS(velocity_controllers::JointTrajectoryController, controller_interface::ControllerBase) +PLUGINLIB_EXPORT_CLASS(effort_controllers::JointTrajectoryController, controller_interface::ControllerBase) +PLUGINLIB_EXPORT_CLASS(pos_vel_controllers::JointTrajectoryController, controller_interface::ControllerBase) +PLUGINLIB_EXPORT_CLASS(pos_vel_acc_controllers::JointTrajectoryController, controller_interface::ControllerBase)