diff --git a/franka_moveit_config/config/panda_ros_controllers.yaml b/franka_moveit_config/config/panda_ros_controllers.yaml index 54a5079f..94b2403b 100644 --- a/franka_moveit_config/config/panda_ros_controllers.yaml +++ b/franka_moveit_config/config/panda_ros_controllers.yaml @@ -3,7 +3,7 @@ controller_manager: update_rate: 1000 # Hz panda_arm_controller: - type: joint_effort_trajectory_controller/JointTrajectoryController + type: joint_trajectory_controller/JointTrajectoryController joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster diff --git a/joint_effort_trajectory_controller/CMakeLists.txt b/joint_effort_trajectory_controller/CMakeLists.txt deleted file mode 100644 index c66fddc2..00000000 --- a/joint_effort_trajectory_controller/CMakeLists.txt +++ /dev/null @@ -1,70 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(joint_effort_trajectory_controller) - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra) -endif() - -find_package(ament_cmake REQUIRED) -find_package(angles REQUIRED) -find_package(controller_interface REQUIRED) -find_package(control_msgs REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(realtime_tools REQUIRED) -find_package(trajectory_msgs REQUIRED) -find_package(control_toolbox REQUIRED) - -add_library(joint_effort_trajectory_controller SHARED - src/joint_trajectory_controller.cpp - src/trajectory.cpp - ) -target_include_directories(joint_effort_trajectory_controller PUBLIC include) -ament_target_dependencies(joint_effort_trajectory_controller - angles - builtin_interfaces - controller_interface - control_msgs - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - trajectory_msgs - control_toolbox - ) - -pluginlib_export_plugin_description_file(controller_interface joint_trajectory_plugin.xml) - -install(DIRECTORY include/ - DESTINATION include - ) - -install(TARGETS joint_effort_trajectory_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - ) - -ament_export_dependencies( - controller_interface - control_msgs - hardware_interface - rclcpp - rclcpp_lifecycle - trajectory_msgs -) -ament_export_include_directories( - include -) -ament_export_libraries( - joint_effort_trajectory_controller -) -ament_package() diff --git a/joint_effort_trajectory_controller/README.md b/joint_effort_trajectory_controller/README.md deleted file mode 100644 index 332955a1..00000000 --- a/joint_effort_trajectory_controller/README.md +++ /dev/null @@ -1,10 +0,0 @@ -**This package is a Foxy version of [this Pull Request](https://github.com/ros-controls/ros2_controllers/pull/225). -It is needed since the joint trajectory controller is currently not able to do torque control.** - -# Original README - -# joint_trajectory_controllers package - -The package implements controllers to interpolate joint's trajectory. - -For detailed documentation check the `docs` folder or [ros2_control documentation](https://ros-controls.github.io/control.ros.org/). diff --git a/joint_effort_trajectory_controller/include/joint_effort_trajectory_controller/joint_trajectory_controller.hpp b/joint_effort_trajectory_controller/include/joint_effort_trajectory_controller/joint_trajectory_controller.hpp deleted file mode 100644 index 25823296..00000000 --- a/joint_effort_trajectory_controller/include/joint_effort_trajectory_controller/joint_trajectory_controller.hpp +++ /dev/null @@ -1,246 +0,0 @@ -// Copyright (c) 2021 ros2_control Development Team -// -// 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_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ -#define JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ - -#include -#include -#include -#include -#include -#include - -#include "control_msgs/action/follow_joint_trajectory.hpp" -#include "control_msgs/msg/joint_trajectory_controller_state.hpp" -#include "control_toolbox/pid.hpp" -#include "controller_interface/controller_interface.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "tolerances.hpp" -#include "visibility_control.h" -#include "rclcpp/duration.hpp" -#include "rclcpp/subscription.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp/timer.hpp" -#include "rclcpp_action/server.hpp" -#include "rclcpp_action/types.hpp" -#include "rclcpp_lifecycle/lifecycle_publisher.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" -#include "realtime_tools/realtime_server_goal_handle.h" -#include "trajectory_msgs/msg/joint_trajectory.hpp" -#include "trajectory_msgs/msg/joint_trajectory_point.hpp" - -using namespace std::chrono_literals; // NOLINT - -namespace rclcpp_action -{ -template -class ServerGoalHandle; -} // namespace rclcpp_action -namespace rclcpp_lifecycle -{ -class State; -} // namespace rclcpp_lifecycle - -namespace joint_trajectory_controller -{ -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -class Trajectory; - -class JointTrajectoryController : public controller_interface::ControllerInterface -{ -public: - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - JointTrajectoryController(); - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - CallbackReturn on_init() override; - - /** - * @brief command_interface_configuration This controller requires the position command - * interfaces for the controlled joints - */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - - /** - * @brief command_interface_configuration This controller requires the position and velocity - * state interfaces for the controlled joints - */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; - -protected: - std::vector joint_names_; - std::vector command_interface_types_; - std::vector state_interface_types_; - - // To reduce number of variables and to make the code shorter the interfaces are ordered in types - // as the following constants - const std::vector allowed_interface_types_ = { - hardware_interface::HW_IF_POSITION, - hardware_interface::HW_IF_VELOCITY, - hardware_interface::HW_IF_ACCELERATION, - hardware_interface::HW_IF_EFFORT, - }; - - // Parameters for some special cases, e.g. hydraulics powered robots - /// Run he controller in open-loop, i.e., read hardware states only when starting controller. - /// This is useful when robot is not exactly following the commanded trajectory. - bool open_loop_control_ = false; - trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; - - // The interfaces are defined as the types in 'allowed_interface_types_' member. - // For convenience, for each type the interfaces are ordered so that i-th position - // matches i-th index in joint_names_ - template - using InterfaceReferences = std::vector>>; - - InterfaceReferences joint_command_interface_; - InterfaceReferences joint_state_interface_; - - bool has_velocity_state_interface_ = false; - bool has_acceleration_state_interface_ = false; - bool has_position_command_interface_ = false; - bool has_velocity_command_interface_ = false; - bool has_acceleration_command_interface_ = false; - bool has_effort_command_interface_ = false; - - /// If true, a velocity feedforward term plus corrective PID term is used - bool use_closed_loop_pid_adapter = false; - std::vector> pids_; - std::chrono::steady_clock::time_point last_update_time_; - std::vector velocity_ff_; - - // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported - bool subscriber_is_active_ = false; - rclcpp::Subscription::SharedPtr joint_command_subscriber_ = - nullptr; - - std::shared_ptr * traj_point_active_ptr_ = nullptr; - std::shared_ptr traj_external_point_ptr_ = nullptr; - std::shared_ptr traj_home_point_ptr_ = nullptr; - std::shared_ptr traj_msg_home_ptr_ = nullptr; - realtime_tools::RealtimeBuffer> - traj_msg_external_point_ptr_; - - // The controller should be in halted state after creation otherwise memory corruption - // TODO(anyone): Is the variable relevant, since we are using lifecycle? - bool is_halted_ = true; - - using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState; - using StatePublisher = realtime_tools::RealtimePublisher; - using StatePublisherPtr = std::unique_ptr; - rclcpp::Publisher::SharedPtr publisher_; - StatePublisherPtr state_publisher_; - - rclcpp::Duration state_publisher_period_ = rclcpp::Duration(20ms); - rclcpp::Time last_state_publish_time_; - - using FollowJTrajAction = control_msgs::action::FollowJointTrajectory; - using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; - using RealtimeGoalHandlePtr = std::shared_ptr; - using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; - - rclcpp_action::Server::SharedPtr action_server_; - bool allow_partial_joints_goal_ = false; - RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any. - rclcpp::TimerBase::SharedPtr goal_handle_timer_; - rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); - - // callbacks for action_server_ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_action::GoalResponse goal_callback( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_action::CancelResponse cancel_callback( - const std::shared_ptr> goal_handle); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void feedback_setup_callback( - std::shared_ptr> goal_handle); - - // fill trajectory_msg so it matches joints controlled by this controller - // positions set to current position, velocities, accelerations and efforts to 0.0 - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void fill_partial_goal( - std::shared_ptr trajectory_msg) const; - // sorts the joints of the incoming message to our local order - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void sort_to_local_joint_order( - std::shared_ptr trajectory_msg); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void add_new_trajectory_msg( - const std::shared_ptr & traj_msg); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool validate_trajectory_point_field( - size_t joint_names_size, const std::vector & vector_field, - const std::string & string_for_vector_field, size_t i, bool allow_empty) const; - - SegmentTolerances default_tolerances_; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void preempt_active_goal(); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void set_hold_position(); - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool reset(); - - using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void publish_state( - const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, - const JointTrajectoryPoint & state_error); - - void read_state_from_hardware(JointTrajectoryPoint & state); - - bool read_state_from_command_interfaces(JointTrajectoryPoint & state); - -private: - bool contains_interface_type( - const std::vector & interface_type_list, const std::string & interface_type); - - void resize_joint_trajectory_point( - trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); -}; - -} // namespace joint_trajectory_controller - -#endif // JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ diff --git a/joint_effort_trajectory_controller/include/joint_effort_trajectory_controller/tolerances.hpp b/joint_effort_trajectory_controller/include/joint_effort_trajectory_controller/tolerances.hpp deleted file mode 100644 index 73e9b116..00000000 --- a/joint_effort_trajectory_controller/include/joint_effort_trajectory_controller/tolerances.hpp +++ /dev/null @@ -1,190 +0,0 @@ -// Copyright 2013 PAL Robotics S.L. -// All rights reserved. -// -// Software License Agreement (BSD License 2.0) -// 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 - -#ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ -#define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ - -#include -#include -#include -#include - -#include "control_msgs/action/follow_joint_trajectory.hpp" - -#include "rclcpp/node.hpp" - -namespace joint_trajectory_controller -{ -/** - * \brief Trajectory state tolerances for position, velocity and acceleration variables. - * - * A tolerance value of zero means that no tolerance will be applied for that variable. - */ -struct StateTolerances -{ - double position = 0.0; - double velocity = 0.0; - double acceleration = 0.0; -}; - -/** - * \brief Trajectory segment tolerances. - */ -struct SegmentTolerances -{ - explicit SegmentTolerances(size_t size = 0) : state_tolerance(size), goal_state_tolerance(size) {} - - /** State tolerances that apply during segment execution. */ - std::vector state_tolerance; - - /** State tolerances that apply for the goal state only.*/ - std::vector goal_state_tolerance; - - /** Extra time after the segment end time allowed to reach the goal state tolerances. */ - double goal_time_tolerance = 0.0; -}; - -/** - * \brief Populate trajectory segment tolerances using data from the ROS node. - * - * It is assumed that the following parameter structure is followed on the provided LifecycleNode. - * Unspecified parameters will take the defaults shown in the comments: - * - * \code - * constraints: - * goal_time: 1.0 # Defaults to zero - * stopped_velocity_tolerance: 0.02 # Defaults to 0.01 - * foo_joint: - * trajectory: 0.05 # Defaults to zero (ie. the tolerance is not enforced) - * goal: 0.03 # Defaults to zero (ie. the tolerance is not enforced) - * bar_joint: - * goal: 0.01 - * \endcode - * - * \param node LifecycleNode where the tolerances are specified. - * \param joint_names Names of joints to look for in the parameter server for a tolerance specification. - * \return Trajectory segment tolerances. - */ -SegmentTolerances get_segment_tolerances( - const rclcpp_lifecycle::LifecycleNode & node, const std::vector & joint_names) -{ - const auto n_joints = joint_names.size(); - SegmentTolerances tolerances; - - // State and goal state tolerances - double stopped_velocity_tolerance = 0.01; - node.get_parameter_or( - "constraints.stopped_velocity_tolerance", stopped_velocity_tolerance, - stopped_velocity_tolerance); - - tolerances.state_tolerance.resize(n_joints); - tolerances.goal_state_tolerance.resize(n_joints); - for (auto i = 0ul; i < n_joints; ++i) - { - const std::string prefix = "constraints." + joint_names[i]; - - node.get_parameter_or( - prefix + ".trajectory", tolerances.state_tolerance[i].position, 0.0); - node.get_parameter_or( - prefix + ".goal", tolerances.goal_state_tolerance[i].position, 0.0); - - auto logger = rclcpp::get_logger("tolerance"); - RCLCPP_DEBUG( - logger, "%s %f", (prefix + ".trajectory").c_str(), tolerances.state_tolerance[i].position); - RCLCPP_DEBUG( - logger, "%s %f", (prefix + ".goal").c_str(), tolerances.goal_state_tolerance[i].position); - - tolerances.goal_state_tolerance[i].velocity = stopped_velocity_tolerance; - } - - // Goal time tolerance - node.get_parameter_or("constraints.goal_time", tolerances.goal_time_tolerance, 0.0); - - return tolerances; -} - -/** - * \param state_error State error to check. - * \param joint_idx Joint index for the state error - * \param state_tolerance State tolerance of joint to check \p state_error against. - * \param show_errors If the joint that violate its tolerance should be output to console. NOT REALTIME if true - * \return True if \p state_error fulfills \p state_tolerance. - */ -inline bool check_state_tolerance_per_joint( - const trajectory_msgs::msg::JointTrajectoryPoint & state_error, int joint_idx, - const StateTolerances & state_tolerance, bool show_errors = false) -{ - using std::abs; - const double error_position = state_error.positions[joint_idx]; - const double error_velocity = - state_error.velocities.empty() ? 0.0 : state_error.velocities[joint_idx]; - const double error_acceleration = - state_error.accelerations.empty() ? 0.0 : state_error.accelerations[joint_idx]; - - const bool is_valid = - !(state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) && - !(state_tolerance.velocity > 0.0 && abs(error_velocity) > state_tolerance.velocity) && - !(state_tolerance.acceleration > 0.0 && abs(error_acceleration) > state_tolerance.acceleration); - - if (is_valid) - { - return true; - } - - if (show_errors) - { - const auto logger = rclcpp::get_logger("tolerances"); - RCLCPP_ERROR(logger, "Path state tolerances failed:"); - - if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) - { - RCLCPP_ERROR( - logger, "Position Error: %f, Position Tolerance: %f", error_position, - state_tolerance.position); - } - if (state_tolerance.velocity > 0.0 && abs(error_velocity) > state_tolerance.velocity) - { - RCLCPP_ERROR( - logger, "Velocity Error: %f, Velocity Tolerance: %f", error_velocity, - state_tolerance.velocity); - } - if ( - state_tolerance.acceleration > 0.0 && abs(error_acceleration) > state_tolerance.acceleration) - { - RCLCPP_ERROR( - logger, "Acceleration Error: %f, Acceleration Tolerance: %f", error_acceleration, - state_tolerance.acceleration); - } - } - return false; -} - -} // namespace joint_trajectory_controller - -#endif // JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ diff --git a/joint_effort_trajectory_controller/include/joint_effort_trajectory_controller/trajectory.hpp b/joint_effort_trajectory_controller/include/joint_effort_trajectory_controller/trajectory.hpp deleted file mode 100644 index 8433abed..00000000 --- a/joint_effort_trajectory_controller/include/joint_effort_trajectory_controller/trajectory.hpp +++ /dev/null @@ -1,171 +0,0 @@ -// Copyright 2017 Open Source Robotics Foundation, 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_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_ -#define JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_ - -#include -#include - -#include "visibility_control.h" -#include "rclcpp/time.hpp" -#include "trajectory_msgs/msg/joint_trajectory.hpp" -#include "trajectory_msgs/msg/joint_trajectory_point.hpp" -namespace joint_trajectory_controller -{ -using TrajectoryPointIter = std::vector::iterator; -using TrajectoryPointConstIter = - std::vector::const_iterator; - -class Trajectory -{ -public: - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - Trajectory(); - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - explicit Trajectory(std::shared_ptr joint_trajectory); - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - explicit Trajectory( - const rclcpp::Time & current_time, - const trajectory_msgs::msg::JointTrajectoryPoint & current_point, - std::shared_ptr joint_trajectory); - - /// Set the point before the trajectory message is replaced/appended - /// Example: if we receive a new trajectory message and it's first point is 0.5 seconds - /// from the current one, we call this function to log the current state, then - /// append/replace the current trajectory - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void set_point_before_trajectory_msg( - const rclcpp::Time & current_time, - const trajectory_msgs::msg::JointTrajectoryPoint & current_point); - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void update(std::shared_ptr joint_trajectory); - - /// Find the segment (made up of 2 points) and its expected state from the - /// containing trajectory. - /** - * Specific case returns for start_segment_itr and end_segment_itr: - * - Sampling before the trajectory start: - * start_segment_itr = begin(), end_segment_itr = begin() - * - Sampling exactly on a point of the trajectory: - * start_segment_itr = iterator where point is, end_segment_itr = iterator after start_segment_itr - * - Sampling between points: - * start_segment_itr = iterator before the sampled point, end_segment_itr = iterator after start_segment_itr - * - Sampling after entire trajectory: - * start_segment_itr = --end(), end_segment_itr = end() - * - Sampling empty msg or before the time given in set_point_before_trajectory_msg() - * return false - */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool sample( - const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & expected_state, - TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr); - - /** - * Do interpolation between 2 states given a time in between their respective timestamps - * - * The start and end states need not necessarily be specified all the way to the acceleration level: - * - If only \b positions are specified, linear interpolation will be used. - * - If \b positions and \b velocities are specified, a cubic spline will be used. - * - If \b positions, \b velocities and \b accelerations are specified, a quintic spline will be used. - * - * If start and end states have different specifications - * (eg. start is position-only, end is position-velocity), the lowest common specification will be used - * (position-only in the example). - * - * \param[in] time_a Time at which the segment state equals \p state_a. - * \param[in] state_a State at \p time_a. - * \param[in] time_b Time at which the segment state equals \p state_b. - * \param[in] state_b State at time \p time_b. - * \param[in] sample_time The time to sample, between time_a and time_b. - * \param[out] output The state at \p sample_time. - */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void interpolate_between_points( - const rclcpp::Time & time_a, const trajectory_msgs::msg::JointTrajectoryPoint & state_a, - const rclcpp::Time & time_b, const trajectory_msgs::msg::JointTrajectoryPoint & state_b, - const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & output); - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - TrajectoryPointConstIter begin() const; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - TrajectoryPointConstIter end() const; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp::Time time_from_start() const; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool has_trajectory_msg() const; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - std::shared_ptr get_trajectory_msg() const - { - return trajectory_msg_; - } - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp::Time get_trajectory_start_time() const { return trajectory_start_time_; } - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool is_sampled_already() const { return sampled_already_; } - -private: - std::shared_ptr trajectory_msg_; - rclcpp::Time trajectory_start_time_; - - rclcpp::Time time_before_traj_msg_; - trajectory_msgs::msg::JointTrajectoryPoint state_before_traj_msg_; - - bool sampled_already_ = false; -}; - -/** - * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p 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) -{ - // t1 must be a subset of t2 - if (t1.size() > t2.size()) - { - return std::vector(); - } - - std::vector mapping_vector(t1.size()); // Return value - for (auto t1_it = t1.begin(); t1_it != t1.end(); ++t1_it) - { - auto t2_it = std::find(t2.begin(), t2.end(), *t1_it); - if (t2.end() == t2_it) - { - return std::vector(); - } - else - { - const size_t t1_dist = std::distance(t1.begin(), t1_it); - const size_t t2_dist = std::distance(t2.begin(), t2_it); - mapping_vector[t1_dist] = t2_dist; - } - } - return mapping_vector; -} - -} // namespace joint_trajectory_controller - -#endif // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_ diff --git a/joint_effort_trajectory_controller/include/joint_effort_trajectory_controller/visibility_control.h b/joint_effort_trajectory_controller/include/joint_effort_trajectory_controller/visibility_control.h deleted file mode 100644 index ec44da7a..00000000 --- a/joint_effort_trajectory_controller/include/joint_effort_trajectory_controller/visibility_control.h +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2017 Open Source Robotics Foundation, 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. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ - -#ifndef JOINT_TRAJECTORY_CONTROLLER__VISIBILITY_CONTROL_H_ -#define JOINT_TRAJECTORY_CONTROLLER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define JOINT_TRAJECTORY_CONTROLLER_EXPORT __attribute__((dllexport)) -#define JOINT_TRAJECTORY_CONTROLLER_IMPORT __attribute__((dllimport)) -#else -#define JOINT_TRAJECTORY_CONTROLLER_EXPORT __declspec(dllexport) -#define JOINT_TRAJECTORY_CONTROLLER_IMPORT __declspec(dllimport) -#endif -#ifdef JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL -#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC JOINT_TRAJECTORY_CONTROLLER_EXPORT -#else -#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC JOINT_TRAJECTORY_CONTROLLER_IMPORT -#endif -#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC_TYPE JOINT_TRAJECTORY_CONTROLLER_PUBLIC -#define JOINT_TRAJECTORY_CONTROLLER_LOCAL -#else -#define JOINT_TRAJECTORY_CONTROLLER_EXPORT __attribute__((visibility("default"))) -#define JOINT_TRAJECTORY_CONTROLLER_IMPORT -#if __GNUC__ >= 4 -#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC __attribute__((visibility("default"))) -#define JOINT_TRAJECTORY_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) -#else -#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC -#define JOINT_TRAJECTORY_CONTROLLER_LOCAL -#endif -#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC_TYPE -#endif - -#endif // JOINT_TRAJECTORY_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/joint_effort_trajectory_controller/joint_trajectory_plugin.xml b/joint_effort_trajectory_controller/joint_trajectory_plugin.xml deleted file mode 100644 index 6042e01f..00000000 --- a/joint_effort_trajectory_controller/joint_trajectory_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - The joint trajectory controller executes joint-space trajectories on a set of joints - - - diff --git a/joint_effort_trajectory_controller/package.xml b/joint_effort_trajectory_controller/package.xml deleted file mode 100644 index 5fa07ab8..00000000 --- a/joint_effort_trajectory_controller/package.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - - joint_effort_trajectory_controller - 0.0.0 - - Joint trajectory controller that can be used with the effort interface. - It is a Foxy version of this PR: https://github.com/ros-controls/ros2_controllers/pull/225. - - Franka Emika GmbH - Apache License 2.0 - - ament_cmake - - angles - pluginlib - realtime_tools - - angles - pluginlib - realtime_tools - - controller_interface - control_msgs - hardware_interface - rclcpp - rclcpp_lifecycle - trajectory_msgs - - ament_cmake_gtest - controller_manager - ros2_control_test_assets - - - ament_cmake - - diff --git a/joint_effort_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_effort_trajectory_controller/src/joint_trajectory_controller.cpp deleted file mode 100644 index 0468ba54..00000000 --- a/joint_effort_trajectory_controller/src/joint_trajectory_controller.cpp +++ /dev/null @@ -1,1261 +0,0 @@ -// Copyright (c) 2021 ros2_control Development Team -// -// 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. - -#include "joint_effort_trajectory_controller/joint_trajectory_controller.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "angles/angles.h" -#include "builtin_interfaces/msg/duration.hpp" -#include "builtin_interfaces/msg/time.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "joint_effort_trajectory_controller/trajectory.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/logging.hpp" -#include "rclcpp/parameter.hpp" -#include "rclcpp/qos.hpp" -#include "rclcpp/qos_event.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp_action/create_server.hpp" -#include "rclcpp_action/server_goal_handle.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" -#include "std_msgs/msg/header.hpp" - -namespace joint_trajectory_controller -{ -JointTrajectoryController::JointTrajectoryController() -: controller_interface::ControllerInterface(), joint_names_({}) -{ - -} - -CallbackReturn JointTrajectoryController::on_init() -{ - try - { - // with the lifecycle node being initialized, we can declare parameters - auto_declare>("joints", joint_names_); - auto_declare>("command_interfaces", command_interface_types_); - auto_declare>("state_interfaces", state_interface_types_); - auto_declare("state_publish_rate", 50.0); - auto_declare("action_monitor_rate", 20.0); - auto_declare("allow_partial_joints_goal", allow_partial_joints_goal_); - auto_declare("open_loop_control", open_loop_control_); - auto_declare("constraints.stopped_velocity_tolerance", 0.01); - auto_declare("constraints.goal_time", 0.0); - for (const auto & joint_name : joint_names_) - { - const std::string prefix = "constraints." + joint_name; - auto_declare(prefix + ".trajectory", 0.0); - auto_declare(prefix + ".goal", 0.0); - } - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - - return CallbackReturn::SUCCESS; -} - -controller_interface::InterfaceConfiguration -JointTrajectoryController::command_interface_configuration() const -{ - controller_interface::InterfaceConfiguration conf; - conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - conf.names.reserve(joint_names_.size() * command_interface_types_.size()); - for (const auto & joint_name : joint_names_) - { - for (const auto & interface_type : command_interface_types_) - { - conf.names.push_back(joint_name + "/" + interface_type); - } - } - return conf; -} - -controller_interface::InterfaceConfiguration -JointTrajectoryController::state_interface_configuration() const -{ - controller_interface::InterfaceConfiguration conf; - conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - conf.names.reserve(joint_names_.size() * state_interface_types_.size()); - for (const auto & joint_name : joint_names_) - { - for (const auto & interface_type : state_interface_types_) - { - conf.names.push_back(joint_name + "/" + interface_type); - } - } - return conf; -} - -controller_interface::return_type JointTrajectoryController::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - return controller_interface::return_type::OK; - } - - auto compute_error_for_joint = [&]( - JointTrajectoryPoint & error, int index, - const JointTrajectoryPoint & current, - const JointTrajectoryPoint & desired) { - // error defined as the difference between current and desired - error.positions[index] = - angles::shortest_angular_distance(current.positions[index], desired.positions[index]); - if (has_velocity_state_interface_ && has_velocity_command_interface_) - { - error.velocities[index] = desired.velocities[index] - current.velocities[index]; - } - if (has_acceleration_state_interface_ && has_acceleration_command_interface_) - { - error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; - } - }; - - // Check if a new external message has been received from nonRT threads - auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); - auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); - if (current_external_msg != *new_external_msg) - { - fill_partial_goal(*new_external_msg); - sort_to_local_joint_order(*new_external_msg); - traj_external_point_ptr_->update(*new_external_msg); - } - - JointTrajectoryPoint state_current, state_desired, state_error; - const auto joint_num = joint_names_.size(); - resize_joint_trajectory_point(state_current, joint_num); - - // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not - // changed, but its value only? - auto assign_interface_from_point = - [&, joint_num](auto & joint_inteface, const std::vector & trajectory_point_interface) { - for (auto index = 0ul; index < joint_num; ++index) - { - joint_inteface[index].get().set_value(trajectory_point_interface[index]); - } - }; - - // current state update - state_current.time_from_start.set__sec(0); - read_state_from_hardware(state_current); - - // currently carrying out a trajectory - if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) - { - - // if sampling the first time, set the point before you sample - if (!(*traj_point_active_ptr_)->is_sampled_already()) - { - - if (open_loop_control_) - { - (*traj_point_active_ptr_) - ->set_point_before_trajectory_msg(get_node()->now(), last_commanded_state_); - - } - else - { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg(get_node()->now(), state_current); - - } - } - resize_joint_trajectory_point(state_error, joint_num); - - // find segment for current timestamp - TrajectoryPointConstIter start_segment_itr, end_segment_itr; - // TODO(anyone): this is kind-of open-loop concept? I am right? - const bool valid_point = - (*traj_point_active_ptr_) - ->sample(get_node()->now(), state_desired, start_segment_itr, end_segment_itr); - - if (valid_point) - { - bool abort = false; - bool outside_goal_tolerance = false; - const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end(); - - // set values for next hardware write() - if (use_closed_loop_pid_adapter && has_effort_command_interface_) - { - const auto period = std::chrono::steady_clock::now() - last_update_time_; - for (auto index = 0ul; index < joint_num; ++index) - { - const double command = - (state_desired.velocities[index] * velocity_ff_[index]) + - pids_[index]->computeCommand( - state_desired.positions[index] - state_current.positions[index], - state_desired.velocities[index] - state_current.velocities[index], period.count()); - joint_command_interface_[3][index].get().set_value(command); - } - last_update_time_ = std::chrono::steady_clock::now(); - } - else - { - if (has_position_command_interface_) - { - assign_interface_from_point(joint_command_interface_[0], state_desired.positions); - } - if (has_velocity_command_interface_) - { - assign_interface_from_point(joint_command_interface_[1], state_desired.velocities); - } - if (has_acceleration_command_interface_) - { - assign_interface_from_point(joint_command_interface_[2], state_desired.accelerations); - } - } - - for (auto index = 0ul; index < joint_num; ++index) - { - compute_error_for_joint(state_error, index, state_current, state_desired); - - if ( - before_last_point && - !check_state_tolerance_per_joint( - state_error, index, default_tolerances_.state_tolerance[index], false)) - { - abort = true; - } - // past the final point, check that we end up inside goal tolerance - if ( - !before_last_point && - !check_state_tolerance_per_joint( - state_error, index, default_tolerances_.goal_state_tolerance[index], false)) - { - outside_goal_tolerance = true; - } - } - - // store command as state when hardware state has tracking offset - last_commanded_state_ = state_desired; - - const auto active_goal = *rt_active_goal_.readFromRT(); - if (active_goal) - { - // send feedback - auto feedback = std::make_shared(); - feedback->header.stamp = get_node()->now(); - feedback->joint_names = joint_names_; - - feedback->actual = state_current; - feedback->desired = state_desired; - feedback->error = state_error; - active_goal->setFeedback(feedback); - - // check abort - if (abort || outside_goal_tolerance) - { - auto result = std::make_shared(); - - if (abort) - { - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); - result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); - } - else if (outside_goal_tolerance) - { - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to goal tolerance violation"); - result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); - } - active_goal->setAborted(result); - // TODO(matthew-reynolds): Need a lock-free write here - // See https://github.com/ros-controls/ros2_controllers/issues/168 - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - - // check goal tolerance - } - else if (!before_last_point) - { - if (!outside_goal_tolerance) - { - auto res = std::make_shared(); - res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); - active_goal->setSucceeded(res); - // TODO(matthew-reynolds): Need a lock-free write here - // See https://github.com/ros-controls/ros2_controllers/issues/168 - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - - RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); - } - else if (default_tolerances_.goal_time_tolerance != 0.0) - { - // if we exceed goal_time_toleralance set it to aborted - const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time(); - const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; - - const double difference = get_node()->now().seconds() - traj_end.seconds(); - if (difference > default_tolerances_.goal_time_tolerance) - { - auto result = std::make_shared(); - result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); - active_goal->setAborted(result); - // TODO(matthew-reynolds): Need a lock-free write here - // See https://github.com/ros-controls/ros2_controllers/issues/168 - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - RCLCPP_WARN( - get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", - difference); - } - } - } - } - } - } else { - // TODO temporary solution to keep the initial position before the first trajectory goal arrives - set_hold_position(); - } - - publish_state(state_desired, state_current, state_error); - return controller_interface::return_type::OK; -} - -void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state) -{ - const auto joint_num = joint_names_.size(); - auto assign_point_from_interface = - [&, joint_num](std::vector & trajectory_point_interface, const auto & joint_inteface) { - for (auto index = 0ul; index < joint_num; ++index) - { - trajectory_point_interface[index] = joint_inteface[index].get().get_value(); - - } - }; - - // Assign values from the hardware - // Position states always exist - assign_point_from_interface(state.positions, joint_state_interface_[0]); - // velocity and acceleration states are optional - if (has_velocity_state_interface_) - { - assign_point_from_interface(state.velocities, joint_state_interface_[1]); - // Acceleration is used only in combination with velocity - if (has_acceleration_state_interface_) - { - assign_point_from_interface(state.accelerations, joint_state_interface_[2]); - } - else - { - // Make empty so the property is ignored during interpolation - state.accelerations.clear(); - } - } - else - { - // Make empty so the property is ignored during interpolation - state.velocities.clear(); - state.accelerations.clear(); - } -} - -bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajectoryPoint & state) -{ - bool has_values = true; - - const auto joint_num = joint_names_.size(); - auto assign_point_from_interface = - [&, joint_num](std::vector & trajectory_point_interface, const auto & joint_inteface) { - for (auto index = 0ul; index < joint_num; ++index) - { - trajectory_point_interface[index] = joint_inteface[index].get().get_value(); - } - }; - - auto interface_has_values = [](const auto & joint_interface) { - return std::find_if(joint_interface.begin(), joint_interface.end(), [](const auto & interface) { - return std::isnan(interface.get().get_value()); - }) == joint_interface.end(); - }; - - // Assign values from the command interfaces as state. Therefore needs check for both. - // Position state interface has to exist always - if (has_position_command_interface_ && interface_has_values(joint_command_interface_[0])) - { - assign_point_from_interface(state.positions, joint_command_interface_[0]); - } - else - { - state.positions.clear(); - has_values = false; - } - // velocity and acceleration states are optional - if (has_velocity_state_interface_) - { - if (has_velocity_command_interface_ && interface_has_values(joint_command_interface_[1])) - { - assign_point_from_interface(state.velocities, joint_command_interface_[1]); - } - else - { - state.velocities.clear(); - has_values = false; - } - } - else - { - state.velocities.clear(); - } - // Acceleration is used only in combination with velocity - if (has_acceleration_state_interface_) - { - if (has_acceleration_command_interface_ && interface_has_values(joint_command_interface_[2])) - { - assign_point_from_interface(state.accelerations, joint_command_interface_[2]); - } - else - { - state.accelerations.clear(); - has_values = false; - } - } - else - { - state.accelerations.clear(); - } - - return has_values; -} - -CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) -{ - const auto logger = get_node()->get_logger(); - - // update parameters - joint_names_ = get_node()->get_parameter("joints").as_string_array(); - - if (!reset()) - { - return CallbackReturn::FAILURE; - } - - if (joint_names_.empty()) - { - RCLCPP_WARN(logger, "'joints' parameter is empty."); - } - - // Specialized, child controllers set interfaces before calling configure function. - if (command_interface_types_.empty()) - { - command_interface_types_ = get_node()->get_parameter("command_interfaces").as_string_array(); - } - - if (command_interface_types_.empty()) - { - RCLCPP_ERROR(logger, "'command_interfaces' parameter is empty."); - return CallbackReturn::FAILURE; - } - - // Check if only allowed interface types are used and initialize storage to avoid memory - // allocation during activation - joint_command_interface_.resize(allowed_interface_types_.size()); - for (const auto & interface : command_interface_types_) - { - auto it = - std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - if (it == allowed_interface_types_.end()) - { - RCLCPP_ERROR(logger, "Command interface type '%s' not allowed!", interface.c_str()); - return CallbackReturn::FAILURE; - } - } - - // Check if command interfaces combination is valid. Valid combinations are: - // 1. effort - // 2. velocity - // 2. position [velocity, [acceleration]] - - // effort can't be combined with other interfaces - if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_EFFORT)) - { - if (command_interface_types_.size() == 1) - { - use_closed_loop_pid_adapter = true; - } - else - { - RCLCPP_ERROR(logger, "'effort' command interface has to be used alone."); - return CallbackReturn::FAILURE; - } - } - - if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_POSITION)) - { - has_position_command_interface_ = true; - } - if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_VELOCITY)) - { - has_velocity_command_interface_ = true; - } - if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_ACCELERATION)) - { - has_acceleration_command_interface_ = true; - } - if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_EFFORT)) - { - has_effort_command_interface_ = true; - } - - if (has_velocity_command_interface_) - { - // if there is only velocity then use also PID adapter - if (command_interface_types_.size() == 1) - { - use_closed_loop_pid_adapter = true; - // TODO(anyone): remove this when implemented - RCLCPP_ERROR(logger, "using 'velocity' command interface alone is not yet implemented yet."); - return CallbackReturn::FAILURE; - // if velocity interface can be used without position if multiple defined - } - else if (!has_position_command_interface_) - { - RCLCPP_ERROR( - logger, - "'velocity' command interface can be used either alone or 'position' " - "interface has to be present."); - return CallbackReturn::FAILURE; - } - // invalid: acceleration is defined and no velocity - } - else if (has_acceleration_command_interface_) - { - RCLCPP_ERROR( - logger, - "'acceleration' command interface can only be used if 'velocity' and " - "'position' interfaces are present"); - return CallbackReturn::FAILURE; - } - - if (use_closed_loop_pid_adapter) - { - for (const auto & joint_name : joint_names_) - { - // Init PID gains from ROS parameter server - const std::string prefix = "gains." + joint_name; - const auto k_p = auto_declare(prefix + ".p", 0.0); - const auto k_i = auto_declare(prefix + ".i", 0.0); - const auto k_d = auto_declare(prefix + ".d", 0.0); - const auto i_clamp = auto_declare(prefix + ".i_clamp", 0.0); - const auto velocity_ff = auto_declare("velocity_ff." + joint_name, 0.0); - // Initialize PID - pids_.push_back(std::make_unique(k_p, k_i, k_d, i_clamp, -i_clamp)); - velocity_ff_.push_back(velocity_ff); - } - } - - // Read always state interfaces from the parameter because they can be used - // independently from the controller's type. - // Specialized, child controllers should set its default value. - state_interface_types_ = get_node()->get_parameter("state_interfaces").as_string_array(); - - if (state_interface_types_.empty()) - { - RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty."); - return CallbackReturn::FAILURE; - } - - if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_EFFORT)) - { - RCLCPP_ERROR(logger, "State interface type 'effort' not allowed!"); - return CallbackReturn::FAILURE; - } - - // Check if only allowed interface types are used and initialize storage to avoid memory - // allocation during activation - // Note: 'effort' storage is also here, but never used. Still, for this is OK. - joint_state_interface_.resize(allowed_interface_types_.size()); - for (const auto & interface : state_interface_types_) - { - auto it = - std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - if (it == allowed_interface_types_.end()) - { - RCLCPP_ERROR(logger, "State interface type '%s' not allowed!", interface.c_str()); - return CallbackReturn::FAILURE; - } - } - - if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_VELOCITY)) - { - has_velocity_state_interface_ = true; - } - if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_ACCELERATION)) - { - has_acceleration_state_interface_ = true; - } - - if (has_velocity_state_interface_) - { - if (!contains_interface_type(state_interface_types_, hardware_interface::HW_IF_POSITION)) - { - RCLCPP_ERROR( - logger, - "'velocity' state interface cannot be used if 'position' interface " - "is missing."); - return CallbackReturn::FAILURE; - } - } - else if (has_acceleration_state_interface_) - { - RCLCPP_ERROR( - logger, - "'acceleration' state interface cannot be used if 'position' and 'velocity' " - "interfaces are not present."); - return CallbackReturn::FAILURE; - } - - auto get_interface_list = [](const std::vector & interface_types) { - std::stringstream ss_command_interfaces; - for (size_t index = 0; index < interface_types.size(); ++index) - { - if (index != 0) - { - ss_command_interfaces << " "; - } - ss_command_interfaces << interface_types[index]; - } - return ss_command_interfaces.str(); - }; - - // Print output so users can be sure the interface setup is correct - RCLCPP_INFO( - logger, "Command interfaces are [%s] and and state interfaces are [%s].", - get_interface_list(command_interface_types_).c_str(), - get_interface_list(state_interface_types_).c_str()); - - default_tolerances_ = get_segment_tolerances(*get_node(), joint_names_); - - // Read parameters customizing controller for special cases - open_loop_control_ = get_node()->get_parameter("open_loop_control").get_value(); - - // subscriber callback - // non realtime - // TODO(karsten): check if traj msg and point time are valid - auto callback = [this](const std::shared_ptr msg) -> void { - if (!validate_trajectory_msg(*msg)) - { - return; - } - - // http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement - // always replace old msg with new one for now - if (subscriber_is_active_) - { - add_new_trajectory_msg(msg); - } - }; - - // TODO(karsten1987): create subscriber with subscription deactivated - joint_command_subscriber_ = get_node()->create_subscription( - "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), callback); - - // TODO(karsten1987): no lifecycle for subscriber yet - // joint_command_subscriber_->on_activate(); - - // State publisher - const double state_publish_rate = get_node()->get_parameter("state_publish_rate").get_value(); - RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", state_publish_rate); - if (state_publish_rate > 0.0) - { - state_publisher_period_ = rclcpp::Duration::from_seconds(1.0 / state_publish_rate); - } - else - { - state_publisher_period_ = rclcpp::Duration::from_seconds(0.0); - } - - publisher_ = get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); - state_publisher_ = std::make_unique(publisher_); - - const auto n_joints = joint_names_.size(); - - 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_.error.positions.resize(n_joints); - if (has_velocity_state_interface_) - { - state_publisher_->msg_.actual.velocities.resize(n_joints); - state_publisher_->msg_.error.velocities.resize(n_joints); - } - if (has_acceleration_state_interface_) - { - state_publisher_->msg_.actual.accelerations.resize(n_joints); - state_publisher_->msg_.error.accelerations.resize(n_joints); - } - state_publisher_->unlock(); - - last_state_publish_time_ = get_node()->now(); - - // action server configuration - allow_partial_joints_goal_ = get_node()->get_parameter("allow_partial_joints_goal").get_value(); - if (allow_partial_joints_goal_) - { - RCLCPP_INFO(logger, "Goals with partial set of joints are allowed"); - } - - const double action_monitor_rate = - get_node()->get_parameter("action_monitor_rate").get_value(); - - RCLCPP_INFO(logger, "Action status changes will be monitored at %.2f Hz.", action_monitor_rate); - action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / action_monitor_rate); - - using namespace std::placeholders; - action_server_ = rclcpp_action::create_server( - get_node()->get_node_base_interface(), get_node()->get_node_clock_interface(), - get_node()->get_node_logging_interface(), get_node()->get_node_waitables_interface(), - std::string(get_node()->get_name()) + "/follow_joint_trajectory", - std::bind(&JointTrajectoryController::goal_callback, this, _1, _2), - std::bind(&JointTrajectoryController::cancel_callback, this, _1), - std::bind(&JointTrajectoryController::feedback_setup_callback, this, _1)); - - return CallbackReturn::SUCCESS; -} - -// Fill ordered_interfaces with references to the matching interfaces -// in the same order as in joint_names -template -bool get_ordered_interfaces( - std::vector & unordered_interfaces, const std::vector & joint_names, - const std::string & interface_type, std::vector> & ordered_interfaces) -{ - for (const auto & joint_name : joint_names) - { - for (auto & interface : unordered_interfaces) - { - if ( - (interface.get_name() == joint_name) && (interface.get_interface_name() == interface_type)) - { - ordered_interfaces.emplace_back(std::ref(interface)); - } - } - } - - return joint_names.size() == ordered_interfaces.size(); -} - -CallbackReturn JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &) -{ - // order all joints in the storage - for (const auto & interface : command_interface_types_) - { - auto it = - std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); - if (!get_ordered_interfaces( - command_interfaces_, joint_names_, interface, joint_command_interface_[index])) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", joint_names_.size(), - interface.c_str(), joint_command_interface_[index].size()); - return CallbackReturn::ERROR; - } - } - for (const auto & interface : state_interface_types_) - { - auto it = - std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); - if (!get_ordered_interfaces( - state_interfaces_, joint_names_, interface, joint_state_interface_[index])) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", joint_names_.size(), - interface.c_str(), joint_state_interface_[index].size()); - return CallbackReturn::ERROR; - } - } - - // Store 'home' pose - traj_msg_home_ptr_ = std::make_shared(); - traj_msg_home_ptr_->header.stamp.sec = 0; - traj_msg_home_ptr_->header.stamp.nanosec = 0; - traj_msg_home_ptr_->points.resize(1); - traj_msg_home_ptr_->points[0].time_from_start.sec = 0; - traj_msg_home_ptr_->points[0].time_from_start.nanosec = 50000000; - traj_msg_home_ptr_->points[0].positions.resize(joint_state_interface_[0].size()); - for (size_t index = 0; index < joint_state_interface_[0].size(); ++index) - { - traj_msg_home_ptr_->points[0].positions[index] = - joint_state_interface_[0][index].get().get_value(); - } - - traj_external_point_ptr_ = std::make_shared(); - traj_home_point_ptr_ = std::make_shared(); - traj_msg_external_point_ptr_.writeFromNonRT( - std::shared_ptr()); - - subscriber_is_active_ = true; - traj_point_active_ptr_ = &traj_external_point_ptr_; - last_state_publish_time_ = get_node()->now(); - - // Initialize current state storage if hardware state has tracking offset - resize_joint_trajectory_point(last_commanded_state_, joint_names_.size()); - read_state_from_hardware(last_commanded_state_); - // Handle restart of controller by reading last_commanded_state_ from commands is - // those are not nan - trajectory_msgs::msg::JointTrajectoryPoint state; - resize_joint_trajectory_point(state, joint_names_.size()); - if (read_state_from_command_interfaces(state)) - { - last_commanded_state_ = state; - } - - // TODO(karsten1987): activate subscriptions of subscriber - return CallbackReturn::SUCCESS; -} - -CallbackReturn JointTrajectoryController::on_deactivate(const rclcpp_lifecycle::State &) -{ - // TODO(anyone): How to halt when using effort commands? - for (auto index = 0ul; index < joint_names_.size(); ++index) - { - joint_command_interface_[0][index].get().set_value( - joint_command_interface_[0][index].get().get_value()); - } - - for (auto index = 0ul; index < allowed_interface_types_.size(); ++index) - { - joint_command_interface_[index].clear(); - joint_state_interface_[index].clear(); - } - release_interfaces(); - - subscriber_is_active_ = false; - - return CallbackReturn::SUCCESS; -} - -CallbackReturn JointTrajectoryController::on_cleanup(const rclcpp_lifecycle::State &) -{ - // go home - traj_home_point_ptr_->update(traj_msg_home_ptr_); - traj_point_active_ptr_ = &traj_home_point_ptr_; - - return CallbackReturn::SUCCESS; -} - -CallbackReturn JointTrajectoryController::on_error(const rclcpp_lifecycle::State &) -{ - if (!reset()) - { - return CallbackReturn::ERROR; - } - return CallbackReturn::SUCCESS; -} - -bool JointTrajectoryController::reset() -{ - subscriber_is_active_ = false; - joint_command_subscriber_.reset(); - - for (const auto & pid : pids_) - { - pid->reset(); - } - - // iterator has no default value - // prev_traj_point_ptr_; - traj_point_active_ptr_ = nullptr; - traj_external_point_ptr_.reset(); - traj_home_point_ptr_.reset(); - traj_msg_home_ptr_.reset(); - - return true; -} - -CallbackReturn JointTrajectoryController::on_shutdown(const rclcpp_lifecycle::State &) -{ - // TODO(karsten1987): what to do? - - return CallbackReturn::SUCCESS; -} - -void JointTrajectoryController::publish_state( - const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, - const JointTrajectoryPoint & state_error) -{ - if (state_publisher_period_.seconds() <= 0.0) - { - return; - } - - if (get_node()->now() < (last_state_publish_time_ + state_publisher_period_)) - { - return; - } - - if (state_publisher_ && state_publisher_->trylock()) - { - last_state_publish_time_ = get_node()->now(); - state_publisher_->msg_.header.stamp = last_state_publish_time_; - state_publisher_->msg_.desired.positions = desired_state.positions; - state_publisher_->msg_.desired.velocities = desired_state.velocities; - state_publisher_->msg_.desired.accelerations = desired_state.accelerations; - state_publisher_->msg_.actual.positions = current_state.positions; - state_publisher_->msg_.error.positions = state_error.positions; - if (has_velocity_state_interface_) - { - state_publisher_->msg_.actual.velocities = current_state.velocities; - state_publisher_->msg_.error.velocities = state_error.velocities; - } - if (has_acceleration_state_interface_) - { - state_publisher_->msg_.actual.accelerations = current_state.accelerations; - state_publisher_->msg_.error.accelerations = state_error.accelerations; - } - - state_publisher_->unlockAndPublish(); - } -} - -rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( - const rclcpp_action::GoalUUID &, std::shared_ptr goal) -{ - RCLCPP_INFO(get_node()->get_logger(), "Received new action goal"); - - // Precondition: Running controller - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new action goals. Controller is not running."); - return rclcpp_action::GoalResponse::REJECT; - } - - if (!validate_trajectory_msg(goal->trajectory)) - { - return rclcpp_action::GoalResponse::REJECT; - } - - RCLCPP_INFO(get_node()->get_logger(), "Accepted new action goal"); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - -rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback( - const std::shared_ptr> goal_handle) -{ - RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); - - // Check that cancel request refers to currently active goal (if any) - const auto active_goal = *rt_active_goal_.readFromNonRT(); - if (active_goal && active_goal->gh_ == goal_handle) - { - // Controller uptime - // Enter hold current position mode - set_hold_position(); - - RCLCPP_DEBUG( - get_node()->get_logger(), "Canceling active action goal because cancel callback received."); - - // Mark the current goal as canceled - auto action_res = std::make_shared(); - active_goal->setCanceled(action_res); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - } - return rclcpp_action::CancelResponse::ACCEPT; -} - -void JointTrajectoryController::feedback_setup_callback( - std::shared_ptr> goal_handle) -{ - // Update new trajectory - { - preempt_active_goal(); - auto traj_msg = - std::make_shared(goal_handle->get_goal()->trajectory); - - add_new_trajectory_msg(traj_msg); - } - - // Update the active goal - RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); - rt_goal->preallocated_feedback_->joint_names = joint_names_; - rt_goal->execute(); - rt_active_goal_.writeFromNonRT(rt_goal); - - // Setup goal status checking timer - goal_handle_timer_ = get_node()->create_wall_timer( - action_monitor_period_.to_chrono(), - std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); -} - -void JointTrajectoryController::fill_partial_goal( - std::shared_ptr trajectory_msg) const -{ - // joint names in the goal are a subset of existing joints, as checked in goal_callback - // so if the size matches, the goal contains all controller joints - if (joint_names_.size() == trajectory_msg->joint_names.size()) - { - return; - } - - trajectory_msg->joint_names.reserve(joint_names_.size()); - - for (auto index = 0ul; index < joint_names_.size(); ++index) - { - { - if ( - std::find( - trajectory_msg->joint_names.begin(), trajectory_msg->joint_names.end(), - joint_names_[index]) != trajectory_msg->joint_names.end()) - { - // joint found on msg - continue; - } - trajectory_msg->joint_names.push_back(joint_names_[index]); - - for (auto & it : trajectory_msg->points) - { - // Assume hold position with 0 velocity and acceleration for missing joints - it.positions.push_back(joint_command_interface_[0][index].get().get_value()); - if (!it.velocities.empty()) - { - it.velocities.push_back(0.0); - } - if (!it.accelerations.empty()) - { - it.accelerations.push_back(0.0); - } - if (!it.effort.empty()) - { - it.effort.push_back(0.0); - } - } - } - } -} - -void JointTrajectoryController::sort_to_local_joint_order( - std::shared_ptr trajectory_msg) -{ - // rearrange all points in the trajectory message based on mapping - std::vector mapping_vector = mapping(trajectory_msg->joint_names, joint_names_); - auto remap = [this]( - const std::vector & to_remap, - const std::vector & mapping) -> std::vector { - if (to_remap.empty()) - { - return to_remap; - } - if (to_remap.size() != mapping.size()) - { - RCLCPP_WARN(get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); - return to_remap; - } - std::vector output; - output.resize(mapping.size(), 0.0); - for (auto index = 0ul; index < mapping.size(); ++index) - { - auto map_index = mapping[index]; - output[map_index] = to_remap[index]; - } - return output; - }; - - for (auto index = 0ul; index < trajectory_msg->points.size(); ++index) - { - trajectory_msg->points[index].positions = - remap(trajectory_msg->points[index].positions, mapping_vector); - - trajectory_msg->points[index].velocities = - remap(trajectory_msg->points[index].velocities, mapping_vector); - - trajectory_msg->points[index].accelerations = - remap(trajectory_msg->points[index].accelerations, mapping_vector); - - trajectory_msg->points[index].effort = - remap(trajectory_msg->points[index].effort, mapping_vector); - } -} - -bool JointTrajectoryController::validate_trajectory_point_field( - size_t joint_names_size, const std::vector & vector_field, - const std::string & string_for_vector_field, size_t i, bool allow_empty) const -{ - if (allow_empty && vector_field.empty()) - { - return true; - } - if (joint_names_size != vector_field.size()) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", - joint_names_size, string_for_vector_field.c_str(), vector_field.size(), i); - return false; - } - return true; -} - -bool JointTrajectoryController::validate_trajectory_msg( - const trajectory_msgs::msg::JointTrajectory & trajectory) const -{ - // If partial joints goals are not allowed, goal should specify all controller joints - if (!allow_partial_joints_goal_) - { - if (trajectory.joint_names.size() != joint_names_.size()) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Joints on incoming trajectory don't match the controller joints."); - return false; - } - } - - if (trajectory.joint_names.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "Empty joint names on incoming trajectory."); - return false; - } - - const auto trajectory_start_time = static_cast(trajectory.header.stamp); - // If the starting time it set to 0.0, it means the controller should start it now. - // Otherwise we check if the trajectory ends before the current time, - // in which case it can be ignored. - if (trajectory_start_time.seconds() != 0.0) - { - auto trajectory_end_time = trajectory_start_time; - for (const auto & p : trajectory.points) - { - trajectory_end_time += p.time_from_start; - } - if (trajectory_end_time < get_node()->now()) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Received trajectory with non zero time start time (%f) that ends on the past (%f)", - trajectory_start_time.seconds(), trajectory_end_time.seconds()); - return false; - } - } - - for (auto i = 0ul; i < trajectory.joint_names.size(); ++i) - { - const std::string & incoming_joint_name = trajectory.joint_names[i]; - - auto it = std::find(joint_names_.begin(), joint_names_.end(), incoming_joint_name); - if (it == joint_names_.end()) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Incoming joint %s doesn't match the controller's joints.", - incoming_joint_name.c_str()); - return false; - } - } - - rclcpp::Duration previous_traj_time(0ms); - for (auto i = 0ul; i < trajectory.points.size(); ++i) - { - if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Time between points %zu and %zu is not strictly increasing, it is %f and %f respectively", - i - 1, i, previous_traj_time.seconds(), - rclcpp::Duration(trajectory.points[i].time_from_start).seconds()); - return false; - } - previous_traj_time = trajectory.points[i].time_from_start; - - const size_t joint_count = trajectory.joint_names.size(); - const auto & points = trajectory.points; - if ( - !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false) || - !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, true) || - !validate_trajectory_point_field( - joint_count, points[i].accelerations, "accelerations", i, true) || - !validate_trajectory_point_field(joint_count, points[i].effort, "effort", i, true)) - { - return false; - } - } - return true; -} - -void JointTrajectoryController::add_new_trajectory_msg( - const std::shared_ptr & traj_msg) -{ - traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); -} - -void JointTrajectoryController::preempt_active_goal() -{ - const auto active_goal = *rt_active_goal_.readFromNonRT(); - if (active_goal) - { - auto action_res = std::make_shared(); - action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); - action_res->set__error_string("Current goal cancelled due to new incoming action."); - active_goal->setCanceled(action_res); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - } -} - -void JointTrajectoryController::set_hold_position() -{ - trajectory_msgs::msg::JointTrajectory msg; - msg.header.stamp = rclcpp::Time(0); - msg.joint_names = joint_names_; - trajectory_msgs::msg::JointTrajectoryPoint point; - for (const auto& joint_position : last_commanded_state_.positions) { - point.velocities.push_back(0); - point.accelerations.push_back(0); - point.positions.push_back(joint_position); - } - msg.points.push_back(point); - - auto traj_msg = std::make_shared(msg); - add_new_trajectory_msg(traj_msg); -} - -bool JointTrajectoryController::contains_interface_type( - const std::vector & interface_type_list, const std::string & interface_type) -{ - return std::find(interface_type_list.begin(), interface_type_list.end(), interface_type) != - interface_type_list.end(); -} - -void JointTrajectoryController::resize_joint_trajectory_point( - trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) -{ - point.positions.resize(size); - if (has_velocity_state_interface_) - { - point.velocities.resize(size); - } - if (has_acceleration_state_interface_) - { - point.accelerations.resize(size); - } -} - -} // namespace joint_trajectory_controller - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - joint_trajectory_controller::JointTrajectoryController, controller_interface::ControllerInterface) diff --git a/joint_effort_trajectory_controller/src/trajectory.cpp b/joint_effort_trajectory_controller/src/trajectory.cpp deleted file mode 100644 index 75b04909..00000000 --- a/joint_effort_trajectory_controller/src/trajectory.cpp +++ /dev/null @@ -1,293 +0,0 @@ -// Copyright 2017 Open Source Robotics Foundation, 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. - -#include "joint_effort_trajectory_controller/trajectory.hpp" - -#include - -#include "hardware_interface/macros.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/time.hpp" -#include "rcppmath/clamp.hpp" -#include "std_msgs/msg/header.hpp" -namespace joint_trajectory_controller -{ -Trajectory::Trajectory() : trajectory_start_time_(0), time_before_traj_msg_(0) {} - -Trajectory::Trajectory(std::shared_ptr joint_trajectory) -: trajectory_msg_(joint_trajectory), - trajectory_start_time_(static_cast(joint_trajectory->header.stamp)) -{ -} - -Trajectory::Trajectory( - const rclcpp::Time & current_time, - const trajectory_msgs::msg::JointTrajectoryPoint & current_point, - std::shared_ptr joint_trajectory) -: trajectory_msg_(joint_trajectory), - trajectory_start_time_(static_cast(joint_trajectory->header.stamp)) -{ - set_point_before_trajectory_msg(current_time, current_point); - update(joint_trajectory); -} - -void Trajectory::set_point_before_trajectory_msg( - const rclcpp::Time & current_time, - const trajectory_msgs::msg::JointTrajectoryPoint & current_point) -{ - time_before_traj_msg_ = current_time; - state_before_traj_msg_ = current_point; -} - -void Trajectory::update(std::shared_ptr joint_trajectory) -{ - trajectory_msg_ = joint_trajectory; - trajectory_start_time_ = static_cast(joint_trajectory->header.stamp); - sampled_already_ = false; -} - -bool Trajectory::sample( - const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & expected_state, - TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr) -{ - THROW_ON_NULLPTR(trajectory_msg_) - expected_state = trajectory_msgs::msg::JointTrajectoryPoint(); - - if (trajectory_msg_->points.empty()) - { - start_segment_itr = end(); - end_segment_itr = end(); - return false; - } - - // first sampling of this trajectory - if (!sampled_already_) - { - if (trajectory_start_time_.seconds() == 0.0) - { - trajectory_start_time_ = sample_time; - } - - sampled_already_ = true; - } - - // sampling before the current point - if (sample_time < time_before_traj_msg_) - { - return false; - } - - // current time hasn't reached traj time of the first point in the msg yet - const auto & first_point_in_msg = trajectory_msg_->points[0]; - const rclcpp::Duration offset = first_point_in_msg.time_from_start; - const rclcpp::Time first_point_timestamp = trajectory_start_time_ + offset; - if (sample_time < first_point_timestamp) - { - const rclcpp::Time t0 = time_before_traj_msg_; - - interpolate_between_points( - t0, state_before_traj_msg_, first_point_timestamp, first_point_in_msg, sample_time, - expected_state); - start_segment_itr = begin(); // no segments before the first - end_segment_itr = begin(); - return true; - } - - // time_from_start + trajectory time is the expected arrival time of trajectory - const auto last_idx = trajectory_msg_->points.size() - 1; - for (auto i = 0ul; i < last_idx; ++i) - { - const auto & point = trajectory_msg_->points[i]; - const auto & next_point = trajectory_msg_->points[i + 1]; - - const rclcpp::Duration t0_offset = point.time_from_start; - const rclcpp::Duration t1_offset = next_point.time_from_start; - const rclcpp::Time t0 = trajectory_start_time_ + t0_offset; - const rclcpp::Time t1 = trajectory_start_time_ + t1_offset; - - if (sample_time >= t0 && sample_time < t1) - { - interpolate_between_points(t0, point, t1, next_point, sample_time, expected_state); - start_segment_itr = begin() + i; - end_segment_itr = begin() + (i + 1); - return true; - } - } - - // whole animation has played out - start_segment_itr = --end(); - end_segment_itr = end(); - expected_state = (*start_segment_itr); - // the trajectories in msg may have empty velocities/accel, so resize them - if (expected_state.velocities.empty()) - { - expected_state.velocities.resize(expected_state.positions.size(), 0.0); - } - if (expected_state.accelerations.empty()) - { - expected_state.accelerations.resize(expected_state.positions.size(), 0.0); - } - return true; -} - -void Trajectory::interpolate_between_points( - const rclcpp::Time & time_a, const trajectory_msgs::msg::JointTrajectoryPoint & state_a, - const rclcpp::Time & time_b, const trajectory_msgs::msg::JointTrajectoryPoint & state_b, - const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & output) -{ - rclcpp::Duration duration_so_far = sample_time - time_a; - rclcpp::Duration duration_btwn_points = time_b - time_a; - - const size_t dim = state_a.positions.size(); - output.positions.resize(dim, 0.0); - output.velocities.resize(dim, 0.0); - output.accelerations.resize(dim, 0.0); - - auto generate_powers = [](int n, double x, double * powers) { - powers[0] = 1.0; - for (int i = 1; i <= n; ++i) - { - powers[i] = powers[i - 1] * x; - } - }; - - bool has_velocity = !state_a.velocities.empty() && !state_b.velocities.empty(); - bool has_accel = !state_a.accelerations.empty() && !state_b.accelerations.empty(); - if (duration_so_far.seconds() < 0.0) - { - duration_so_far = rclcpp::Duration::from_seconds(0.0); - has_velocity = has_accel = false; - } - if (duration_so_far.seconds() > duration_btwn_points.seconds()) - { - duration_so_far = duration_btwn_points; - has_velocity = has_accel = false; - } - - double t[6]; - generate_powers(5, duration_so_far.seconds(), t); - - if (!has_velocity && !has_accel) - { - // do linear interpolation - for (size_t i = 0; i < dim; ++i) - { - double start_pos = state_a.positions[i]; - double end_pos = state_b.positions[i]; - - double coefficients[2] = {0.0, 0.0}; - coefficients[0] = start_pos; - if (duration_btwn_points.seconds() != 0.0) - { - coefficients[1] = (end_pos - start_pos) / duration_btwn_points.seconds(); - } - - output.positions[i] = t[0] * coefficients[0] + t[1] * coefficients[1]; - output.velocities[i] = t[0] * coefficients[1]; - } - } - else if (has_velocity && !has_accel) - { - // do cubic interpolation - double T[4]; - generate_powers(3, duration_btwn_points.seconds(), T); - - for (size_t i = 0; i < dim; ++i) - { - double start_pos = state_a.positions[i]; - double start_vel = state_a.velocities[i]; - double end_pos = state_b.positions[i]; - double end_vel = state_b.velocities[i]; - - double coefficients[4] = {0.0, 0.0, 0.0, 0.0}; - coefficients[0] = start_pos; - coefficients[1] = start_vel; - if (duration_btwn_points.seconds() != 0.0) - { - coefficients[2] = - (-3.0 * start_pos + 3.0 * end_pos - 2.0 * start_vel * T[1] - end_vel * T[1]) / T[2]; - coefficients[3] = - (2.0 * start_pos - 2.0 * end_pos + start_vel * T[1] + end_vel * T[1]) / T[3]; - } - - output.positions[i] = t[0] * coefficients[0] + t[1] * coefficients[1] + - t[2] * coefficients[2] + t[3] * coefficients[3]; - output.velocities[i] = - t[0] * coefficients[1] + t[1] * 2.0 * coefficients[2] + t[2] * 3.0 * coefficients[3]; - output.accelerations[i] = t[0] * 2.0 * coefficients[2] + t[1] * 6.0 * coefficients[3]; - } - } - else if (has_velocity && has_accel) - { - // do quintic interpolation - double T[6]; - generate_powers(5, duration_btwn_points.seconds(), T); - - for (size_t i = 0; i < dim; ++i) - { - double start_pos = state_a.positions[i]; - double start_vel = state_a.velocities[i]; - double start_acc = state_a.accelerations[i]; - double end_pos = state_b.positions[i]; - double end_vel = state_b.velocities[i]; - double end_acc = state_b.accelerations[i]; - - double coefficients[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - coefficients[0] = start_pos; - coefficients[1] = start_vel; - coefficients[2] = 0.5 * start_acc; - if (duration_btwn_points.seconds() != 0.0) - { - coefficients[3] = (-20.0 * start_pos + 20.0 * end_pos - 3.0 * start_acc * T[2] + - end_acc * T[2] - 12.0 * start_vel * T[1] - 8.0 * end_vel * T[1]) / - (2.0 * T[3]); - coefficients[4] = (30.0 * start_pos - 30.0 * end_pos + 3.0 * start_acc * T[2] - - 2.0 * end_acc * T[2] + 16.0 * start_vel * T[1] + 14.0 * end_vel * T[1]) / - (2.0 * T[4]); - coefficients[5] = (-12.0 * start_pos + 12.0 * end_pos - start_acc * T[2] + end_acc * T[2] - - 6.0 * start_vel * T[1] - 6.0 * end_vel * T[1]) / - (2.0 * T[5]); - } - - output.positions[i] = t[0] * coefficients[0] + t[1] * coefficients[1] + - t[2] * coefficients[2] + t[3] * coefficients[3] + - t[4] * coefficients[4] + t[5] * coefficients[5]; - output.velocities[i] = t[0] * coefficients[1] + t[1] * 2.0 * coefficients[2] + - t[2] * 3.0 * coefficients[3] + t[3] * 4.0 * coefficients[4] + - t[4] * 5.0 * coefficients[5]; - output.accelerations[i] = t[0] * 2.0 * coefficients[2] + t[1] * 6.0 * coefficients[3] + - t[2] * 12.0 * coefficients[4] + t[3] * 20.0 * coefficients[5]; - } - } -} - -TrajectoryPointConstIter Trajectory::begin() const -{ - THROW_ON_NULLPTR(trajectory_msg_) - - return trajectory_msg_->points.begin(); -} - -TrajectoryPointConstIter Trajectory::end() const -{ - THROW_ON_NULLPTR(trajectory_msg_) - - return trajectory_msg_->points.end(); -} - -rclcpp::Time Trajectory::time_from_start() const { return trajectory_start_time_; } - -bool Trajectory::has_trajectory_msg() const { return trajectory_msg_.get() != nullptr; } - -} // namespace joint_trajectory_controller