From 800cdf956fd43e49a11ebad056226cbeff9eaff8 Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Thu, 12 Aug 2021 16:32:09 -0700 Subject: [PATCH 1/2] Copied under review implementation of ros_controllerS --- CMakeLists.txt | 3 + ...ward_joint_group_command_controller_impl.h | 306 ++++++++++++++++++ .../forward_joint_group_command_controller.h | 150 +++++++++ .../joint_group_effort_controller.h | 61 ++++ .../joint_group_position_controller.h | 62 ++++ .../joint_group_velocity_controller.h | 63 ++++ package.xml | 2 +- src/joint_group_effort_controller.cpp | 65 ++++ src/joint_group_position_controller.cpp | 70 ++++ src/joint_group_velocity_controller.cpp | 65 ++++ 10 files changed, 846 insertions(+), 1 deletion(-) create mode 100644 include/rewd_controllers/detail/forward_joint_group_command_controller_impl.h create mode 100644 include/rewd_controllers/forward_joint_group_command_controller.h create mode 100644 include/rewd_controllers/joint_group_effort_controller.h create mode 100644 include/rewd_controllers/joint_group_position_controller.h create mode 100644 include/rewd_controllers/joint_group_velocity_controller.h create mode 100644 src/joint_group_effort_controller.cpp create mode 100644 src/joint_group_position_controller.cpp create mode 100644 src/joint_group_velocity_controller.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b258306..9933bca 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,6 +67,9 @@ add_library("${PROJECT_NAME}" SHARED src/MoveUntilTouchTopicController.cpp src/FTThresholdClient.cpp src/FTThresholdServer.cpp + src/joint_group_position_controller.cpp + src/joint_group_velocity_controller.cpp + src/joint_group_effort_controller.cpp ) target_link_libraries("${PROJECT_NAME}" ${DART_LIBRARIES} diff --git a/include/rewd_controllers/detail/forward_joint_group_command_controller_impl.h b/include/rewd_controllers/detail/forward_joint_group_command_controller_impl.h new file mode 100644 index 0000000..3cec4ba --- /dev/null +++ b/include/rewd_controllers/detail/forward_joint_group_command_controller_impl.h @@ -0,0 +1,306 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2021, Personal Robotics Lab +// +// 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 Personal Robotics Lab 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 Ethan Kroll Gordon + +#pragma once + + +namespace forward_command_controller +{ + +namespace internal +{ +// TODO: create a utils file? +/** + * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \t2 indices. + * If \p t1 is "{C, B}" and \p t2 is "{A, B, C, D}", the associated mapping vector is + * "{2, 1}". + */ +template +inline std::vector mapping(const T& t1, const T& t2) +{ + typedef unsigned int SizeType; + + // t1 must be a subset of t2 + if (t1.size() > t2.size()) {return std::vector();} + + std::vector mapping_vector(t1.size()); // Return value + for (typename T::const_iterator t1_it = t1.begin(); t1_it != t1.end(); ++t1_it) + { + typename T::const_iterator t2_it = std::find(t2.begin(), t2.end(), *t1_it); + if (t2.end() == t2_it) {return std::vector();} + else + { + const SizeType t1_dist = std::distance(t1.begin(), t1_it); + const SizeType t2_dist = std::distance(t2.begin(), t2_it); + mapping_vector[t1_dist] = t2_dist; + } + } + return mapping_vector; +} + +inline 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 +bool ForwardJointGroupCommandController::init(HardwareInterface* hw, + ros::NodeHandle& n) +{ + // Cache controller node handle + controller_nh_ = n; + + // Controller name + name_ = internal::getLeafNamespace(controller_nh_); + + // 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."); + + // Initialize controlled joints + std::string param_name = "joints"; + if(!n.getParam(param_name, joint_names_)) + { + ROS_ERROR_STREAM("Failed to getParam '" << param_name << "' (namespace: " << n.getNamespace() << ")."); + return false; + } + n_joints_ = joint_names_.size(); + + if(n_joints_ == 0){ + ROS_ERROR_STREAM("List of joint names is empty."); + return false; + } + for(unsigned int i=0; igetHandle(joint_names_[i])); + } + catch (const hardware_interface::HardwareInterfaceException& e) + { + ROS_ERROR_STREAM("Exception thrown: " << e.what()); + return false; + } + } + + commands_buffer_.writeFromNonRT(std::vector(n_joints_, 0.0)); + default_commands_.resize(n_joints_); + + // ROS API: Subscribed topics + sub_command_ = n.subscribe("command", 1, &ForwardJointGroupCommandController::commandCB, this); + + // ROS API: Action interface + action_server_.reset(new ActionServer(controller_nh_, "joint_group_command", + boost::bind(&ForwardJointGroupCommandController::goalCB, this, _1), + boost::bind(&ForwardJointGroupCommandController::cancelCB, this, _1), + false)); + action_server_->start(); + return true; +} + +template +void ForwardJointGroupCommandController::preemptActiveGoal() +{ + RealtimeGoalHandlePtr current_active_goal(rt_active_goal_); + + // Cancel any goal timeout + goal_duration_timer_.stop(); + + // 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 +void ForwardJointGroupCommandController::commandCB(const std_msgs::Float64MultiArrayConstPtr& msg) +{ + // Preconditions + if (!this->isRunning()) + { + ROS_ERROR_STREAM_NAMED(name_, "Can't accept new commands. Controller is not running."); + return; + } + + if (!msg) + { + ROS_WARN_STREAM_NAMED(name_, "Received null-pointer message, skipping."); + return; + } + + if(msg->data.size()!=n_joints_) + { + ROS_ERROR_STREAM_NAMED(name_, "Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!"); + return; + } + + commands_buffer_.writeFromNonRT(msg->data); + preemptActiveGoal(); +} + +template +void ForwardJointGroupCommandController::setGoal(GoalHandle gh, + std::vector command) +{ + ROS_DEBUG_STREAM_NAMED(name_,"Received new action goal"); + pr_control_msgs::JointGroupCommandResult result; + + // Preconditions + if (!this->isRunning()) + { + result.error_string = "Can't accept new action goals. Controller is not running."; + ROS_ERROR_STREAM_NAMED(name_, result.error_string); + result.error_code = pr_control_msgs::JointGroupCommandResult::INVALID_GOAL; + gh.setRejected(result); + return; + } + + + if (gh.getGoal()->joint_names.size() != command.size()) { + result.error_string = "Size of command must match size of joint_names."; + ROS_ERROR_STREAM_NAMED(name_, result.error_string); + result.error_code = pr_control_msgs::JointGroupCommandResult::INVALID_GOAL; + 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()->joint_names, joint_names_); + + if (mapping_vector.size() != gh.getGoal()->joint_names.size()) + { + result.error_string = "Joints on incoming goal don't match the controller joints."; + ROS_ERROR_STREAM_NAMED(name_, result.error_string); + result.error_code = pr_control_msgs::JointGroupCommandResult::INVALID_JOINTS; + gh.setRejected(result); + return; + } + + // update new command + RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh)); + std::vector< double > new_commands = default_commands_; + for(int i = 0; i < mapping_vector.size(); i++) { + new_commands[mapping_vector[i]] = command[i]; + } + rt_goal->preallocated_feedback_->joint_names = joint_names_; + commands_buffer_.writeFromNonRT(new_commands); + + // 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(); + + // Setup goal timeout + if (gh.getGoal()->command.time_from_start > ros::Duration()) { + goal_duration_timer_ = controller_nh_.createTimer(gh.getGoal()->command.time_from_start, + &ForwardJointGroupCommandController::timeoutCB, + this, + true); + goal_duration_timer_.start(); + } +} + +template +void ForwardJointGroupCommandController::timeoutCB(const ros::TimerEvent& event) +{ + RealtimeGoalHandlePtr current_active_goal(rt_active_goal_); + + // Check that timeout refers to currently active goal (if any) + if (current_active_goal) { + ROS_DEBUG_NAMED(name_, "Active action goal reached requested timeout."); + + // Give sub-classes option to update default_commands_ + updateDefaultCommand(); + commands_buffer_.writeFromNonRT(default_commands_); + + // Marks the current goal as succeeded + rt_active_goal_.reset(); + current_active_goal->gh_.setSucceeded(); + } +} + +template +void ForwardJointGroupCommandController::cancelCB(GoalHandle gh) +{ + RealtimeGoalHandlePtr current_active_goal(rt_active_goal_); + + // Check that cancel request refers to currently active goal + if (current_active_goal && current_active_goal->gh_ == gh) + { + ROS_DEBUG_NAMED(name_, "Canceling active action goal because cancel callback recieved from actionlib."); + + // Give sub-classes option to update default_commands_ + updateDefaultCommand(); + commands_buffer_.writeFromNonRT(default_commands_); + + preemptActiveGoal(); + } +} + +template +void ForwardJointGroupCommandController::setActionFeedback(const ros::Time& time) +{ + RealtimeGoalHandlePtr current_active_goal(rt_active_goal_); + if (!current_active_goal) + { + return; + } + + current_active_goal->preallocated_feedback_->header.stamp = time; + current_active_goal->preallocated_feedback_->desired = current_active_goal->gh_.getGoal()->command; + current_active_goal->preallocated_feedback_->actual.positions.clear(); + current_active_goal->preallocated_feedback_->actual.velocities.clear(); + current_active_goal->preallocated_feedback_->actual.effort.clear(); + for (auto j : joints_) + { + current_active_goal->preallocated_feedback_->actual.positions.push_back(j.getPosition()); + current_active_goal->preallocated_feedback_->actual.velocities.push_back(j.getVelocity()); + current_active_goal->preallocated_feedback_->actual.effort.push_back(j.getEffort()); + } + + current_active_goal->setFeedback( current_active_goal->preallocated_feedback_ ); +} + +} // namespace diff --git a/include/rewd_controllers/forward_joint_group_command_controller.h b/include/rewd_controllers/forward_joint_group_command_controller.h new file mode 100644 index 0000000..1c8e949 --- /dev/null +++ b/include/rewd_controllers/forward_joint_group_command_controller.h @@ -0,0 +1,150 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2014, Fraunhofer IPA + * All rights reserved. + * + * 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 the Willow Garage 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. + *********************************************************************/ + +#pragma once + + +#include +#include + +#include +#include + +// actionlib +#include + +// ROS messages +#include +#include +#include + +// ros_controls +#include +#include +#include + + +namespace forward_command_controller +{ + +/** + * \brief Forward command controller for a set of joints. + * + * This class forwards the command signal down to a set of joints. + * Command signal and joint hardware interface are of the same type, e.g. effort commands for an effort-controlled + * joint. + * + * \tparam T Type implementing the JointCommandInterface. + * + * \section ROS interface + * + * \param type hardware interface type. + * \param joints Names of the joints to control. + * + * Subscribes to: + * - \b command (std_msgs::Float64MultiArray) : The joint commands to apply. + */ +template +class ForwardJointGroupCommandController: public controller_interface::Controller +{ +public: + ForwardJointGroupCommandController() {} + ~ForwardJointGroupCommandController() {sub_command_.shutdown();} + + bool init(HardwareInterface* hw, ros::NodeHandle &n); + + void starting(const ros::Time& /*time*/); + void stopping(const ros::Time& /*time*/) {preemptActiveGoal();} + void update(const ros::Time& time, const ros::Duration& /*period*/) + { + std::vector & commands = *commands_buffer_.readFromRT(); + for(unsigned int i=0; i ActionServer; + typedef std::shared_ptr ActionServerPtr; + typedef ActionServer::GoalHandle GoalHandle; + typedef realtime_tools::RealtimeServerGoalHandle RealtimeGoalHandle; + typedef boost::shared_ptr RealtimeGoalHandlePtr; + + realtime_tools::RealtimeBuffer > commands_buffer_; + std::vector default_commands_; // Defaults to 0 on init, can override in starting() + unsigned int n_joints_; + + std::vector< std::string > joint_names_; ///< Controlled joint names. + std::vector< hardware_interface::JointHandle > joints_; ///< Handle to controlled joint. + std::string name_; ///< Controller name. + RealtimeGoalHandlePtr rt_active_goal_; ///< Currently active action goal, if any. + + // ROS API + ros::NodeHandle controller_nh_; + ros::Subscriber sub_command_; + ActionServerPtr action_server_; + ros::Timer goal_handle_timer_; + ros::Timer goal_duration_timer_; + ros::Duration action_monitor_period_; + + // Callback to call setGoal. Override with specific command + void goalCB(GoalHandle gh); + void setGoal(GoalHandle gh, std::vector command); + + // General callbacks + void cancelCB(GoalHandle gh); + void timeoutCB(const ros::TimerEvent& event); + void preemptActiveGoal(); + void commandCB(const std_msgs::Float64MultiArrayConstPtr& msg); + + // Defaults to no change in default command + void updateDefaultCommand(); + +private: + /** + * @brief Updates the pre-allocated feedback of the current active goal (if any) + * based on the current state values. + * + * @note This function is NOT thread safe but intended to be used in the + * update-function. + */ + void setActionFeedback(const ros::Time& time); +}; + +} // namespace + +#include diff --git a/include/rewd_controllers/joint_group_effort_controller.h b/include/rewd_controllers/joint_group_effort_controller.h new file mode 100644 index 0000000..f9a0435 --- /dev/null +++ b/include/rewd_controllers/joint_group_effort_controller.h @@ -0,0 +1,61 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2014, Fraunhofer IPA + * All rights reserved. + * + * 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 the Willow Garage 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. + *********************************************************************/ + +#pragma once + + +#include + +namespace effort_controllers +{ + +/** + * \brief Forward command controller for a set of effort controlled joints (torque or force). + * + * This class forwards the commanded efforts down to a set of joints. + * + * \section ROS interface + * + * \param type Must be "JointGroupEffortController". + * \param joints List of names of the joints to control. + * + * Subscribes to: + * - \b command (std_msgs::Float64MultiArray) : The joint efforts to apply + */ +typedef forward_command_controller::ForwardJointGroupCommandController + JointGroupEffortController; +} diff --git a/include/rewd_controllers/joint_group_position_controller.h b/include/rewd_controllers/joint_group_position_controller.h new file mode 100644 index 0000000..b1609cf --- /dev/null +++ b/include/rewd_controllers/joint_group_position_controller.h @@ -0,0 +1,62 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2014, Fraunhofer IPA + * All rights reserved. + * + * 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 the Willow Garage 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. + *********************************************************************/ + +#pragma once + + +#include +#include + +namespace position_controllers +{ + +/** + * \brief Forward command controller for a set of position controlled joints (linear or angular). + * + * This class forwards the commanded positions down to a set of joints. + * + * \section ROS interface + * + * \param type Must be "JointGroupPositionController". + * \param joints List of names of the joints to control. + * + * Subscribes to: + * - \b command (std_msgs::Float64MultiArray) : The joint positions to apply + */ +typedef forward_command_controller::ForwardJointGroupCommandController + JointGroupPositionController; +} diff --git a/include/rewd_controllers/joint_group_velocity_controller.h b/include/rewd_controllers/joint_group_velocity_controller.h new file mode 100644 index 0000000..e01ad40 --- /dev/null +++ b/include/rewd_controllers/joint_group_velocity_controller.h @@ -0,0 +1,63 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2014, Fraunhofer IPA + * All rights reserved. + * + * 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 the Willow Garage 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. + *********************************************************************/ + +#pragma once + + +#include +#include + +namespace velocity_controllers +{ + +/** + * \brief Forward command controller for a set of velocity controlled joints (linear or angular). + * + * This class forwards the commanded velocities down to a set of joints. + * + * \section ROS interface + * + * \param type Must be "JointGroupVelocityController". + * \param joints List of names of the joints to control. + * + * Subscribes to: + * - \b command (std_msgs::Float64MultiArray) : The joint velocities to apply + */ +typedef forward_command_controller::ForwardJointGroupCommandController + JointGroupVelocityController; + +} diff --git a/package.xml b/package.xml index 524d4f6..072e86e 100644 --- a/package.xml +++ b/package.xml @@ -3,7 +3,7 @@ rewd_controllers 0.0.0 The ROS Effort With Dynamics Controllers package - Clint Liddick + Ethan K. Gordon BSD https://github.com/personalrobotics/rewd_controllers Clint Liddick diff --git a/src/joint_group_effort_controller.cpp b/src/joint_group_effort_controller.cpp new file mode 100644 index 0000000..5661418 --- /dev/null +++ b/src/joint_group_effort_controller.cpp @@ -0,0 +1,65 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2014, Fraunhofer IPA + * All rights reserved. + * + * 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 the Willow Garage 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. + *********************************************************************/ + +#include +#include + +template +void forward_command_controller::ForwardJointGroupCommandController::starting(const ros::Time& time) +{ + // Start controller with 0.0 efforts + commands_buffer_.readFromRT()->assign(n_joints_, 0.0); +} + +template +void forward_command_controller::ForwardJointGroupCommandController::updateDefaultCommand() +{ + // Set defaults to 0 + for(unsigned int i=0; i +void forward_command_controller::ForwardJointGroupCommandController::goalCB(GoalHandle gh) +{ + // Set as position command + setGoal(gh, gh.getGoal()->command.effort); +} + +PLUGINLIB_EXPORT_CLASS(effort_controllers::JointGroupEffortController,controller_interface::ControllerBase) diff --git a/src/joint_group_position_controller.cpp b/src/joint_group_position_controller.cpp new file mode 100644 index 0000000..371ddcb --- /dev/null +++ b/src/joint_group_position_controller.cpp @@ -0,0 +1,70 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2014, Fraunhofer IPA + * All rights reserved. + * + * 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 the Willow Garage 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. + *********************************************************************/ + +#include +#include + +template +void forward_command_controller::ForwardJointGroupCommandController::starting(const ros::Time& time) +{ + // Start controller with current joint positions + std::vector & commands = *commands_buffer_.readFromRT(); + for(unsigned int i=0; i +void forward_command_controller::ForwardJointGroupCommandController::updateDefaultCommand() +{ + // Set default to current position + for(unsigned int i=0; i +void forward_command_controller::ForwardJointGroupCommandController::goalCB(GoalHandle gh) +{ + // Set as position command + setGoal(gh, gh.getGoal()->command.positions); +} + +PLUGINLIB_EXPORT_CLASS(position_controllers::JointGroupPositionController,controller_interface::ControllerBase) diff --git a/src/joint_group_velocity_controller.cpp b/src/joint_group_velocity_controller.cpp new file mode 100644 index 0000000..a625e5f --- /dev/null +++ b/src/joint_group_velocity_controller.cpp @@ -0,0 +1,65 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2014, Fraunhofer IPA + * All rights reserved. + * + * 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 the Willow Garage 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. + *********************************************************************/ + +#include +#include + +template +void forward_command_controller::ForwardJointGroupCommandController::starting(const ros::Time& time) +{ + // Start controller with 0.0 velocities + commands_buffer_.readFromRT()->assign(n_joints_, 0.0); +} + +template +void forward_command_controller::ForwardJointGroupCommandController::updateDefaultCommand() +{ + // Set default to 0 + for(unsigned int i=0; i +void forward_command_controller::ForwardJointGroupCommandController::goalCB(GoalHandle gh) +{ + // Set as position command + setGoal(gh, gh.getGoal()->command.velocities); +} + +PLUGINLIB_EXPORT_CLASS(velocity_controllers::JointGroupVelocityController,controller_interface::ControllerBase) From e10a201c4b6459a4c9f36b0d0bfdb9412110a047 Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Thu, 19 Aug 2021 10:01:47 -0700 Subject: [PATCH 2/2] Address PR comment --- .../detail/forward_joint_group_command_controller_impl.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/include/rewd_controllers/detail/forward_joint_group_command_controller_impl.h b/include/rewd_controllers/detail/forward_joint_group_command_controller_impl.h index 3cec4ba..73cebd5 100644 --- a/include/rewd_controllers/detail/forward_joint_group_command_controller_impl.h +++ b/include/rewd_controllers/detail/forward_joint_group_command_controller_impl.h @@ -102,6 +102,9 @@ bool ForwardJointGroupCommandController::init(HardwareInterfa ROS_ERROR_STREAM("List of joint names is empty."); return false; } + + // Clear joints_ first in case this is called twice + joints_.clear(); for(unsigned int i=0; i