├── .gitlab-ci.yml ├── cartesian_ros_control ├── CMakeLists.txt ├── doc │ ├── resources │ │ ├── fzi_logo.png │ │ └── ur_logo.jpg │ └── cartesian_ros_control.png └── package.xml ├── cartesian_control_msgs ├── msg │ ├── CartesianTolerance.msg │ ├── CartesianTrajectory.msg │ ├── CartesianPosture.msg │ └── CartesianTrajectoryPoint.msg ├── action │ └── FollowCartesianTrajectory.action ├── package.xml ├── README.md └── CMakeLists.txt ├── pass_through_controllers ├── test │ ├── launch │ │ ├── robot.launch │ │ ├── setup.launch │ │ └── hw_interface.launch │ ├── hw_interface │ │ ├── config.yaml │ │ ├── controllers.yaml │ │ ├── control_node.cpp │ │ └── hw_interface.h │ ├── cfg │ │ └── SpeedScaling.cfg │ ├── pass_through_controllers.test │ ├── test_forward_cartesian_trajectory.py │ └── test_forward_joint_trajectory.py ├── pass_through_controllers_plugin.xml ├── src │ └── pass_through_controllers.cpp ├── package.xml └── README.md ├── cartesian_trajectory_controller ├── doc │ └── resources │ │ └── rviz_visualize_cartesian_trajectories.png ├── test │ ├── cartesian_trajectory_controller.test │ ├── config.yaml │ ├── controllers.yaml │ ├── setup.xml │ ├── test_controller.py │ └── visualization.rviz ├── ik_solver_example_plugin.xml ├── cartesian_trajectory_controller_plugin.xml ├── src │ ├── ik_solver_example.cpp │ └── cartesian_trajectory_controller.cpp ├── include │ ├── inverse_kinematics │ │ ├── ik_solver_example.h │ │ └── ik_solver_base.h │ └── cartesian_trajectory_controller │ │ └── cartesian_trajectory_controller.h ├── package.xml └── README.md ├── twist_controller ├── cfg │ └── TwistController.cfg ├── twist_controller_plugin.xml ├── README.md ├── package.xml ├── include │ └── twist_controller │ │ └── twist_controller.h ├── src │ └── twist_controller.cpp └── CMakeLists.txt ├── .clang-tidy ├── cartesian_trajectory_interpolation ├── README.md ├── package.xml ├── src │ ├── cartesian_trajectory.cpp │ └── cartesian_state.cpp ├── include │ └── cartesian_trajectory_interpolation │ │ ├── cartesian_state.h │ │ └── cartesian_trajectory.h └── test │ ├── cartesian_state_test.cpp │ ├── cartesian_trajectory_segment_test.cpp │ └── cartesian_trajectory_test.cpp ├── .github └── workflows │ └── industrial_ci_action.yml ├── cartesian_interface ├── CMakeLists.txt ├── include │ └── cartesian_interface │ │ ├── speed_scaling_interface.h │ │ ├── cartesian_command_interface.h │ │ └── cartesian_state_handle.h ├── package.xml ├── test │ ├── cartesian_state_interface_test.cpp │ └── cartesian_command_interface_test.cpp └── README.md ├── LICENSE ├── LICENSES └── license.cpp ├── .clang-format └── README.md /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | include: 2 | - project: 'continuous_integration/ci_scripts' 3 | ref: master 4 | file: '/gitlab-ci-yml/catkin_pipeline.yml' 5 | -------------------------------------------------------------------------------- /cartesian_ros_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(cartesian_ros_control) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /cartesian_ros_control/doc/resources/fzi_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fzi-forschungszentrum-informatik/cartesian_ros_control/HEAD/cartesian_ros_control/doc/resources/fzi_logo.png -------------------------------------------------------------------------------- /cartesian_ros_control/doc/resources/ur_logo.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fzi-forschungszentrum-informatik/cartesian_ros_control/HEAD/cartesian_ros_control/doc/resources/ur_logo.jpg -------------------------------------------------------------------------------- /cartesian_ros_control/doc/cartesian_ros_control.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fzi-forschungszentrum-informatik/cartesian_ros_control/HEAD/cartesian_ros_control/doc/cartesian_ros_control.png -------------------------------------------------------------------------------- /cartesian_control_msgs/msg/CartesianTolerance.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Vector3 position_error 2 | geometry_msgs/Vector3 orientation_error 3 | geometry_msgs/Twist twist_error 4 | geometry_msgs/Accel acceleration_error 5 | -------------------------------------------------------------------------------- /cartesian_control_msgs/msg/CartesianTrajectory.msg: -------------------------------------------------------------------------------- 1 | # header.frame_id is the frame in which all data from CartesianTrajectoryPoint[] is given 2 | Header header 3 | CartesianTrajectoryPoint[] points 4 | string controlled_frame 5 | -------------------------------------------------------------------------------- /pass_through_controllers/test/launch/robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /pass_through_controllers/test/hw_interface/config.yaml: -------------------------------------------------------------------------------- 1 | # HW interface configuration 2 | 3 | control_rate: 125 4 | 5 | joints: 6 | - joint1 7 | - joint2 8 | - joint3 9 | - joint4 10 | - joint5 11 | - joint6 12 | -------------------------------------------------------------------------------- /cartesian_trajectory_controller/doc/resources/rviz_visualize_cartesian_trajectories.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fzi-forschungszentrum-informatik/cartesian_ros_control/HEAD/cartesian_trajectory_controller/doc/resources/rviz_visualize_cartesian_trajectories.png -------------------------------------------------------------------------------- /cartesian_control_msgs/msg/CartesianPosture.msg: -------------------------------------------------------------------------------- 1 | # Posture joint names may reflect a subset of all available joints (empty posture definitions are 2 | # also possible). The length of posture_joint_names and posture_joint_values have to be equal. 3 | 4 | string[] posture_joint_names 5 | float64[] posture_joint_values 6 | -------------------------------------------------------------------------------- /cartesian_control_msgs/msg/CartesianTrajectoryPoint.msg: -------------------------------------------------------------------------------- 1 | duration time_from_start 2 | geometry_msgs/Pose pose 3 | geometry_msgs/Twist twist 4 | geometry_msgs/Accel acceleration 5 | # A more suitable datatype would be good, see https://github.com/ros/common_msgs/issues/137 6 | geometry_msgs/Accel jerk 7 | CartesianPosture posture 8 | -------------------------------------------------------------------------------- /cartesian_trajectory_controller/test/cartesian_trajectory_controller.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /twist_controller/cfg/TwistController.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "twist_controller" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("twist_gain", double_t, 0, "A factor with which the input twist is multiplied", 1.0, 0, 100) 9 | 10 | exit(gen.generate(PACKAGE, "twist_controller", "TwistController")) 11 | -------------------------------------------------------------------------------- /pass_through_controllers/test/cfg/SpeedScaling.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "pass_through_controllers" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("speed_scaling", double_t, 0, "Speed scaling in the oem controller", 1.0, 0.0, 1.0) 9 | 10 | exit(gen.generate(PACKAGE, "pass_through_controllers", "SpeedScaling")) 11 | -------------------------------------------------------------------------------- /pass_through_controllers/test/launch/setup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /twist_controller/twist_controller_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | The TwistController receives a twist message and sends that directly to a Cartesian robot interface 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /cartesian_trajectory_controller/ik_solver_example_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | An example IK solver for joint-based Cartesian trajectory controllers 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /cartesian_trajectory_controller/test/config.yaml: -------------------------------------------------------------------------------- 1 | # This is a configuration for the ros_control_boilerplate package 2 | 3 | generic_hw_control_loop: 4 | loop_hz: 100 5 | cycle_time_error_threshold: 0.03 6 | 7 | # Settings for ros_control hardware interface 8 | hardware_interface: 9 | ref_frame_id: 'base_link' 10 | frame_id: 'tool0' 11 | sim_control_mode: 0 12 | joints: 13 | - joint1 14 | - joint2 15 | - joint3 16 | - joint4 17 | - joint5 18 | - joint6 19 | -------------------------------------------------------------------------------- /pass_through_controllers/test/pass_through_controllers.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /cartesian_control_msgs/action/FollowCartesianTrajectory.action: -------------------------------------------------------------------------------- 1 | CartesianTrajectory trajectory 2 | CartesianTolerance path_tolerance 3 | CartesianTolerance goal_tolerance 4 | duration goal_time_tolerance 5 | 6 | --- 7 | 8 | int32 error_code 9 | int32 SUCCESSFUL = 0 10 | int32 INVALID_GOAL = -1 # e.g. illegal quaternions in poses 11 | int32 INVALID_JOINTS = -2 12 | int32 OLD_HEADER_TIMESTAMP = -3 13 | int32 PATH_TOLERANCE_VIOLATED = -4 14 | int32 GOAL_TOLERANCE_VIOLATED = -5 15 | int32 INVALID_POSTURE = -6 16 | 17 | string error_string 18 | 19 | --- 20 | 21 | Header header 22 | string tcp_frame 23 | CartesianTrajectoryPoint desired 24 | CartesianTrajectoryPoint actual 25 | CartesianTrajectoryPoint error 26 | -------------------------------------------------------------------------------- /.clang-tidy: -------------------------------------------------------------------------------- 1 | Checks: '-*,readability-identifier-naming' 2 | CheckOptions: 3 | - { key: readability-identifier-naming.NamespaceCase, value: lower_case } 4 | - { key: readability-identifier-naming.ClassCase, value: CamelCase } 5 | - { key: readability-identifier-naming.PrivateMemberSuffix, value: _ } 6 | - { key: readability-identifier-naming.StructCase, value: CamelCase } 7 | - { key: readability-identifier-naming.FunctionCase, value: camelBack } 8 | - { key: readability-identifier-naming.VariableCase, value: lower_case } 9 | - { key: readability-identifier-naming.GlobalVariablePrefix, value: g_ } 10 | - { key: readability-identifier-naming.GlobalConstantCase, value: UPPER_CASE } 11 | -------------------------------------------------------------------------------- /pass_through_controllers/pass_through_controllers_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | This controller forwards joint trajectory goals to the robot for interpolation 8 | 9 | 10 | 11 | 14 | 15 | This controller forwards Cartesian trajectory goals to the robot for interpolation 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /cartesian_control_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cartesian_control_msgs 4 | 0.0.1 5 | Cartesian trajectory execution interface. 6 | 7 | Felix Exner 8 | 9 | Apache-2.0 10 | 11 | 12 | 13 | 14 | Felix Exner 15 | 16 | 17 | catkin 18 | 19 | message_generation 20 | message_runtime 21 | 22 | actionlib_msgs 23 | geometry_msgs 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /cartesian_trajectory_controller/test/controllers.yaml: -------------------------------------------------------------------------------- 1 | cartesian_trajectory_controller: 2 | type: "position_controllers/CartesianTrajectoryController" 3 | base: "base_link" 4 | tip: "tool0" 5 | 6 | joints: 7 | - joint1 8 | - joint2 9 | - joint3 10 | - joint4 11 | - joint5 12 | - joint6 13 | 14 | cartesian_trajectory_publisher: 15 | type: "cartesian_trajectory_publisher/CartesianTrajectoryPublisher" 16 | base: "base_link" 17 | tip: "tool0" 18 | joints: 19 | - joint1 20 | - joint2 21 | - joint3 22 | - joint4 23 | - joint5 24 | - joint6 25 | 26 | joint_trajectory_controller: 27 | type: "position_controllers/JointTrajectoryController" 28 | joints: 29 | - joint1 30 | - joint2 31 | - joint3 32 | - joint4 33 | - joint5 34 | - joint6 35 | 36 | 37 | joint_state_controller: 38 | type: joint_state_controller/JointStateController 39 | publish_rate: 50 40 | -------------------------------------------------------------------------------- /cartesian_trajectory_interpolation/README.md: -------------------------------------------------------------------------------- 1 | # Cartesian Trajectory Interpolation 2 | 3 | A library for building and interpolating Cartesian-space trajectories for 4 | control. 5 | 6 | *** 7 | 11 | 12 | 13 | rosin_logo 15 | 16 | 17 | Supported by ROSIN - ROS-Industrial Quality-Assured Robot Software Components. 18 | More information: rosin-project.eu 19 | 20 | eu_flag 22 | 23 | This project has received funding from the European Union’s Horizon 2020 24 | research and innovation programme under grant agreement no. 732287. 25 | 26 | -------------------------------------------------------------------------------- /.github/workflows/industrial_ci_action.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | industrial_ci: 7 | strategy: 8 | matrix: 9 | ros_distro: [ kinetic, melodic ] 10 | ros_repo: [ main, testing ] 11 | env: 12 | CCACHE_DIR: "${{ github.workspace }}/.ccache" 13 | runs-on: ubuntu-latest 14 | steps: 15 | - uses: actions/checkout@v1 16 | - name: ccache cache 17 | uses: actions/cache@v2 18 | with: 19 | path: ${{ env.CCACHE_DIR }} 20 | # we always want the ccache cache to be persisted, as we cannot easily 21 | # determine whether dependencies have changed, and ccache will manage 22 | # updating the cache for us. Adding 'run_id' to the key will force an 23 | # upload at the end of the job. 24 | key: ccache-${{ matrix.ros_distro }}-${{ matrix.ros_repo }}-${{github.run_id}} 25 | restore-keys: | 26 | ccache-${{ matrix.ros_distro }}-${{ matrix.ros_repo }} 27 | - uses: 'ros-industrial/industrial_ci@master' 28 | env: 29 | ROS_DISTRO: ${{ matrix.ros_distro }} 30 | ROS_REPO: ${{ matrix.ros_repo }} 31 | -------------------------------------------------------------------------------- /pass_through_controllers/test/hw_interface/controllers.yaml: -------------------------------------------------------------------------------- 1 | # A controller to forward joint trajectories to a native robot controller for 2 | # interpolation. 3 | forward_joint_trajectories: 4 | type: "joint_trajectory_controllers/PassThroughController" 5 | joints: 6 | - joint1 7 | - joint2 8 | - joint3 9 | - joint4 10 | - joint5 11 | - joint6 12 | 13 | # A controller to forward Cartesian trajectories to a native robot controller 14 | # for interpolation. 15 | forward_cartesian_trajectories: 16 | type: "cartesian_trajectory_controllers/PassThroughController" 17 | joints: 18 | - joint1 19 | - joint2 20 | - joint3 21 | - joint4 22 | - joint5 23 | - joint6 24 | 25 | # For testing resource conflicts with joint trajectory forwarding 26 | conflicting_joint_controller: 27 | type: "position_controllers/JointTrajectoryController" 28 | joints: 29 | - joint1 30 | - joint2 31 | - joint3 32 | - joint4 33 | - joint5 34 | - joint6 35 | 36 | # For testing resource conflicts with Cartesian trajectory forwarding 37 | conflicting_cartesian_controller: 38 | type: "position_controllers/JointTrajectoryController" 39 | joints: 40 | - joint1 41 | - joint2 42 | - joint3 43 | - joint4 44 | - joint5 45 | - joint6 46 | -------------------------------------------------------------------------------- /twist_controller/README.md: -------------------------------------------------------------------------------- 1 | # Twist Controller 2 | 3 | A Cartesian ROS-controller for commanding target twists to a robot. 4 | 5 | This controller makes use of a `TwistCommandInterface` to set a user specified 6 | twist message as reference for robot control. 7 | 8 | ## Controller .yaml 9 | A possible configuration looks like this: 10 | 11 | ```yaml 12 | twist_controller: 13 | type: cartesian_ros_controllers/TwistController 14 | 15 | # The controller uses this identifier to get the according command handle 16 | # from the hardware interface. 17 | frame_id: "endpoint_to_control" 18 | 19 | joints: 20 | - joint1 21 | - joint2 22 | - joint3 23 | - joint4 24 | - joint5 25 | - joint6 26 | ``` 27 | 28 | *** 29 | 33 | 34 | 35 | rosin_logo 37 | 38 | 39 | Supported by ROSIN - ROS-Industrial Quality-Assured Robot Software Components. 40 | More information: rosin-project.eu 41 | 42 | eu_flag 44 | 45 | This project has received funding from the European Union’s Horizon 2020 46 | research and innovation programme under grant agreement no. 732287. 47 | -------------------------------------------------------------------------------- /cartesian_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cartesian_interface) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | geometry_msgs 12 | hardware_interface 13 | roscpp 14 | ) 15 | 16 | 17 | catkin_package( 18 | INCLUDE_DIRS include 19 | #LIBRARIES 20 | CATKIN_DEPENDS 21 | geometry_msgs 22 | hardware_interface 23 | roscpp 24 | #DEPENDS system_lib 25 | ) 26 | 27 | ########### 28 | ## Build ## 29 | ########### 30 | 31 | include_directories( 32 | include 33 | ${catkin_INCLUDE_DIRS} 34 | ) 35 | 36 | ############# 37 | ## Install ## 38 | ############# 39 | 40 | ## Mark cpp header files for installation 41 | install(DIRECTORY include/${PROJECT_NAME}/ 42 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 43 | FILES_MATCHING PATTERN "*.h" 44 | PATTERN ".svn" EXCLUDE 45 | ) 46 | 47 | ############# 48 | ## Testing ## 49 | ############# 50 | 51 | if(CATKIN_ENABLE_TESTING) 52 | catkin_add_gtest(cartesian_state_interface_test test/cartesian_state_interface_test.cpp) 53 | target_link_libraries(cartesian_state_interface_test ${catkin_LIBRARIES}) 54 | 55 | catkin_add_gtest(cartesian_command_interface_test test/cartesian_command_interface_test.cpp) 56 | target_link_libraries(cartesian_command_interface_test ${catkin_LIBRARIES}) 57 | endif() 58 | -------------------------------------------------------------------------------- /cartesian_control_msgs/README.md: -------------------------------------------------------------------------------- 1 | # Cartesian Control Messages 2 | A new take on Cartesian trajectory control in ROS. 3 | 4 | Cartesian trajectory definitions have long been a complicated and highly-discussed topic in ROS. 5 | This set of messages and definitions is the result of an in-depth investigation 6 | of current approaches in both academia and industry. 7 | 8 | While its arguably impossible to cover every detail, to meet all requirements 9 | of such an interface, the goal is to nevertheless provide a consensus that 10 | covers _most_ use cases and let's the community start using Cartesian trajectory control in ROS. 11 | 12 | Please have a look at the [design document][1] for further details on decisions and rationals. 13 | 14 | 15 | [1]: https://github.com/fzi-forschungszentrum-informatik/fzi_robot_interface_proposal 16 | *** 17 | 21 | 22 | 23 | rosin_logo 25 | 26 | 27 | Supported by ROSIN - ROS-Industrial Quality-Assured Robot Software Components. 28 | More information: rosin-project.eu 29 | 30 | eu_flag 32 | 33 | This project has received funding from the European Union’s Horizon 2020 34 | research and innovation programme under grant agreement no. 732287. 35 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The 3-Clause BSD License 2 | 3 | Copyright 2020 FZI Research Center for Information Technology 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /pass_through_controllers/test/hw_interface/control_node.cpp: -------------------------------------------------------------------------------- 1 | // -- BEGIN LICENSE BLOCK ----------------------------------------------------- 2 | // -- END LICENSE BLOCK ------------------------------------------------------- 3 | 4 | //----------------------------------------------------------------------------- 5 | /*!\file control_node.cpp 6 | * 7 | * \author Stefan Scherzinger 8 | * \date 2020/09/09 9 | * 10 | */ 11 | //----------------------------------------------------------------------------- 12 | 13 | 14 | // Ros 15 | #include 16 | 17 | // Ros control 18 | #include 19 | 20 | // This project 21 | #include "hw_interface.h" 22 | #include "ros/time.h" 23 | 24 | 25 | int main(int argc, char** argv) 26 | { 27 | ros::init(argc, argv, "hw_interface_example"); 28 | ros::NodeHandle nh; 29 | examples::HWInterface hw_interface; 30 | controller_manager::ControllerManager controller_manager(&hw_interface); 31 | 32 | int control_rate; 33 | if (!nh.getParam("control_rate", control_rate)) 34 | { 35 | control_rate = 125; 36 | ROS_INFO_STREAM("Failed to load 'control_rate' from parameter server. Using default " << control_rate); 37 | } 38 | 39 | // Control cycle for the robot 40 | ros::Rate rate(control_rate); 41 | 42 | ros::AsyncSpinner spinner(2); 43 | spinner.start(); 44 | 45 | while (ros::ok()) 46 | { 47 | auto period = rate.expectedCycleTime(); // use nominal cycle 48 | 49 | hw_interface.read(ros::Time::now(), period); 50 | hw_interface.write(ros::Time::now(), period); 51 | controller_manager.update(ros::Time::now(), period); 52 | 53 | rate.sleep(); 54 | } 55 | 56 | 57 | return 0; 58 | } 59 | -------------------------------------------------------------------------------- /cartesian_ros_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cartesian_ros_control 4 | 0.0.0 5 | ros_control in Cartesian space meta package. 6 | 7 | 8 | 9 | 10 | Stefan Scherzinger 11 | Felix Exner 12 | 13 | 14 | 15 | 16 | 17 | BSD 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | Felix Exner 30 | 31 | 32 | catkin 33 | 34 | 35 | cartesian_interface 36 | twist_controller 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /LICENSES/license.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2020 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | -------------------------------------------------------------------------------- /cartesian_trajectory_controller/test/setup.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 28 | 29 | 35 | 36 | 37 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /cartesian_control_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cartesian_control_msgs) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | actionlib_msgs 12 | geometry_msgs 13 | message_generation 14 | ) 15 | 16 | ################################################ 17 | ## Declare ROS messages, services and actions ## 18 | ################################################ 19 | 20 | add_message_files( 21 | FILES 22 | CartesianPosture.msg 23 | CartesianTolerance.msg 24 | CartesianTrajectory.msg 25 | CartesianTrajectoryPoint.msg 26 | ) 27 | 28 | # add_service_files( 29 | # FILES 30 | # Service1.srv 31 | # Service2.srv 32 | # ) 33 | 34 | add_action_files( 35 | FILES 36 | FollowCartesianTrajectory.action 37 | ) 38 | 39 | ## Generate added messages and services with any dependencies listed here 40 | generate_messages( 41 | DEPENDENCIES 42 | actionlib_msgs 43 | geometry_msgs 44 | ) 45 | 46 | 47 | ################################### 48 | ## catkin specific configuration ## 49 | ################################### 50 | ## The catkin_package macro generates cmake config files for your package 51 | ## Declare things to be passed to dependent projects 52 | ## INCLUDE_DIRS: uncomment this if your package contains header files 53 | ## LIBRARIES: libraries you create in this project that dependent projects also need 54 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 55 | ## DEPENDS: system dependencies of this project that dependent projects also need 56 | catkin_package( 57 | # INCLUDE_DIRS include 58 | # LIBRARIES cartesian_control_msgs 59 | CATKIN_DEPENDS 60 | actionlib_msgs 61 | geometry_msgs 62 | message_runtime 63 | # DEPENDS system_lib 64 | ) 65 | -------------------------------------------------------------------------------- /pass_through_controllers/test/launch/hw_interface.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -2 4 | ConstructorInitializerIndentWidth: 2 5 | AlignEscapedNewlinesLeft: false 6 | AlignTrailingComments: true 7 | AllowAllParametersOfDeclarationOnNextLine: false 8 | AllowShortIfStatementsOnASingleLine: false 9 | AllowShortLoopsOnASingleLine: false 10 | AllowShortFunctionsOnASingleLine: None 11 | AllowShortLoopsOnASingleLine: false 12 | AlwaysBreakTemplateDeclarations: true 13 | AlwaysBreakBeforeMultilineStrings: false 14 | BreakBeforeBinaryOperators: false 15 | BreakBeforeTernaryOperators: false 16 | BreakConstructorInitializersBeforeComma: true 17 | BinPackParameters: true 18 | ColumnLimit: 120 19 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 20 | DerivePointerBinding: false 21 | PointerBindsToType: true 22 | ExperimentalAutoDetectBinPacking: false 23 | IndentCaseLabels: true 24 | MaxEmptyLinesToKeep: 1 25 | NamespaceIndentation: None 26 | ObjCSpaceBeforeProtocolList: true 27 | PenaltyBreakBeforeFirstCallParameter: 19 28 | PenaltyBreakComment: 60 29 | PenaltyBreakString: 1 30 | PenaltyBreakFirstLessLess: 1000 31 | PenaltyExcessCharacter: 1000 32 | PenaltyReturnTypeOnItsOwnLine: 90 33 | SpacesBeforeTrailingComments: 2 34 | Cpp11BracedListStyle: false 35 | Standard: Auto 36 | IndentWidth: 2 37 | TabWidth: 2 38 | UseTab: Never 39 | IndentFunctionDeclarationAfterType: false 40 | SpacesInParentheses: false 41 | SpacesInAngles: false 42 | SpaceInEmptyParentheses: false 43 | SpacesInCStyleCastParentheses: false 44 | SpaceAfterControlStatementKeyword: true 45 | SpaceBeforeAssignmentOperators: true 46 | ContinuationIndentWidth: 4 47 | SortIncludes: false 48 | SpaceAfterCStyleCast: false 49 | 50 | # Configure each individual brace in BraceWrapping 51 | BreakBeforeBraces: Custom 52 | 53 | # Control of individual brace wrapping cases 54 | BraceWrapping: { 55 | AfterCaseLabel: 'true' 56 | AfterClass: 'true' 57 | AfterControlStatement: 'true' 58 | AfterEnum : 'true' 59 | AfterFunction : 'true' 60 | AfterNamespace : 'true' 61 | AfterStruct : 'true' 62 | AfterUnion : 'true' 63 | BeforeCatch : 'true' 64 | BeforeElse : 'true' 65 | IndentBraces : 'false' 66 | } 67 | ... 68 | -------------------------------------------------------------------------------- /cartesian_interface/include/cartesian_interface/speed_scaling_interface.h: -------------------------------------------------------------------------------- 1 | // -- BEGIN LICENSE BLOCK ---------------------------------------------- 2 | // Copyright 2019 FZI Forschungszentrum Informatik 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // -- END LICENSE BLOCK ------------------------------------------------ 16 | 17 | //---------------------------------------------------------------------- 18 | /*!\file 19 | * 20 | * \author Felix Exner exner@fzi.de 21 | * \date 2019-04-17 22 | * 23 | */ 24 | //---------------------------------------------------------------------- 25 | 26 | 27 | //---------------------------------------------------------------------- 28 | // 29 | // Note: This file was modified from the original. 30 | // 31 | //---------------------------------------------------------------------- 32 | 33 | #pragma once 34 | 35 | #include 36 | 37 | namespace hardware_interface 38 | { 39 | class SpeedScalingHandle 40 | { 41 | public: 42 | SpeedScalingHandle() : name_(""), scaling_factor_(0){}; 43 | SpeedScalingHandle(const std::string& name, const double* scaling_factor) 44 | : name_(name), scaling_factor_(scaling_factor){}; 45 | virtual ~SpeedScalingHandle() = default; 46 | 47 | std::string getName() const 48 | { 49 | return name_; 50 | } 51 | 52 | const double* getScalingFactor() const 53 | { 54 | return scaling_factor_; 55 | } 56 | 57 | private: 58 | std::string name_; 59 | const double* scaling_factor_; 60 | }; 61 | /** \brief Hardware interface to support reading the speed scaling factor. */ 62 | class SpeedScalingInterface : public hardware_interface::HardwareResourceManager 63 | { 64 | }; 65 | } // namespace hardware_interface 66 | -------------------------------------------------------------------------------- /cartesian_trajectory_controller/cartesian_trajectory_controller_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | The CartesianTrajectoryController spline-interpolates a list of Cartesian waypoints in time. 8 | This variant executes the Cartesian trajectories through sampling for a pose-based control interface on the robot. 9 | 10 | 11 | 12 | 22 | 23 | 26 | 27 | The CartesianTrajectoryController spline-interpolates a list of Cartesian waypoints in time. 28 | This variant executes the Cartesian trajectories through sampling Cartesian poses and uses a Levenberg-Marquardt Inverse Kinematics solver for the mapping to joint positions. 29 | 30 | 31 | 32 | 42 | 43 | 46 | 47 | A read-only ROS controller for publishing Cartesian 48 | trajectories in the form of reference poses and twists. 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /cartesian_trajectory_controller/src/ik_solver_example.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2021 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | 31 | //----------------------------------------------------------------------------- 32 | /*!\file ik_solver_example.cpp 33 | * 34 | * \author Stefan Scherzinger 35 | * \date 2021/05/12 36 | * 37 | */ 38 | //----------------------------------------------------------------------------- 39 | 40 | #include 41 | 42 | // Pluginlib 43 | #include 44 | 45 | /** 46 | * \class cartesian_ros_control::ExampleIKSolver 47 | * 48 | * You may explicitly specify this solver with \a "example_solver" as \a 49 | * ik_solver in the controllers.yaml configuration file: 50 | * 51 | * \code{.yaml} 52 | * : 53 | * type: "position_controllers/CartesianTrajectoryController" 54 | * ik_solver: "example_solver" 55 | * ... 56 | * \endcode 57 | * 58 | */ 59 | PLUGINLIB_EXPORT_CLASS(cartesian_ros_control::ExampleIKSolver, cartesian_ros_control::IKSolver) 60 | -------------------------------------------------------------------------------- /pass_through_controllers/src/pass_through_controllers.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2021 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | 31 | 32 | //----------------------------------------------------------------------------- 33 | /*!\file pass_through_controllers.cpp 34 | * 35 | * \author Stefan Scherzinger 36 | * \date 2020/10/13 37 | * 38 | */ 39 | //----------------------------------------------------------------------------- 40 | 41 | // Pluginlib 42 | #include 43 | 44 | // Project 45 | #include 46 | 47 | // Exports 48 | 49 | namespace joint_trajectory_controllers { 50 | 51 | using PassThroughController = 52 | trajectory_controllers::PassThroughController; 53 | } 54 | 55 | namespace cartesian_trajectory_controllers { 56 | 57 | using PassThroughController = 58 | trajectory_controllers::PassThroughController; 59 | } 60 | 61 | PLUGINLIB_EXPORT_CLASS(joint_trajectory_controllers::PassThroughController, 62 | controller_interface::ControllerBase) 63 | 64 | PLUGINLIB_EXPORT_CLASS(cartesian_trajectory_controllers::PassThroughController, 65 | controller_interface::ControllerBase) 66 | -------------------------------------------------------------------------------- /cartesian_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cartesian_interface 4 | 0.0.0 5 | Defines a hardware interface to send Cartesian commands to a robot hardware and read 6 | Cartesian states. 7 | 8 | 9 | 10 | 11 | Felix Exner 12 | Stefan Scherzinger 13 | 14 | 15 | 16 | 17 | 18 | BSD 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | catkin 55 | 56 | roscpp 57 | hardware_interface 58 | geometry_msgs 59 | 60 | rosunit 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /cartesian_trajectory_interpolation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cartesian_trajectory_interpolation 4 | 0.0.0 5 | Cartesian trajectory interpolation as a standalone library 6 | 7 | 8 | 9 | 10 | scherzin 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | rospy 53 | roscpp 54 | cartesian_control_msgs 55 | tf2_eigen 56 | joint_trajectory_controller 57 | rospy 58 | roscpp 59 | cartesian_control_msgs 60 | tf2_eigen 61 | joint_trajectory_controller 62 | rospy 63 | roscpp 64 | 65 | rosunit 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /twist_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | twist_controller 4 | 0.0.0 5 | 6 | A ros_control controller accepting Cartesian twist messages in order to move a robot manipulator. 7 | It uses a Cartesian interface to the robot, so that the robot hardware takes care about 8 | doing the inverse kinematics. This could be used e.g. for visual servoing applications. 9 | 10 | 11 | 12 | 13 | Stefan Scherzinger 14 | Felix Exner 15 | 16 | 17 | 18 | 19 | 20 | BSD 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | Felix Exner 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | catkin 57 | cartesian_interface 58 | dynamic_reconfigure 59 | geometry_msgs 60 | hardware_interface 61 | realtime_tools 62 | roscpp 63 | 64 | controller_interface 65 | controller_interface 66 | controller_interface 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /cartesian_trajectory_controller/include/inverse_kinematics/ik_solver_example.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2021 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | 31 | //----------------------------------------------------------------------------- 32 | /*!\file ik_solver_example.h 33 | * 34 | * \author Stefan Scherzinger 35 | * \date 2021/05/12 36 | * 37 | */ 38 | //----------------------------------------------------------------------------- 39 | 40 | #pragma once 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | namespace cartesian_ros_control 48 | { 49 | /** 50 | * @brief A wrapper around KDL's Levenberg Marquardt solver 51 | * 52 | * This is the default Inverse Kinematics (IK) solver for the 53 | * cartesian_trajectory_controller. 54 | */ 55 | class ExampleIKSolver : public IKSolver 56 | { 57 | public: 58 | ExampleIKSolver(){}; 59 | ~ExampleIKSolver(){}; 60 | 61 | /** 62 | * @brief Initialize the solver 63 | * 64 | * Only the kinematics chain is used. 65 | * 66 | */ 67 | bool init(const KDL::Chain& robot_chain, ros::NodeHandle&, ros::NodeHandle&) override 68 | { 69 | robot_chain_ = robot_chain; 70 | lma_solver_ = std::make_unique(robot_chain_); 71 | return true; 72 | }; 73 | 74 | /** 75 | * @brief Compute Inverse Kinematics with KDL's Levenberg Marquardt solver. 76 | * 77 | */ 78 | virtual int cartToJnt(const KDL::JntArray& q_init, const KDL::Frame& goal, KDL::JntArray& q_out) override 79 | { 80 | return lma_solver_->CartToJnt(q_init, goal, q_out); 81 | }; 82 | 83 | private: 84 | std::unique_ptr lma_solver_; 85 | KDL::Chain robot_chain_; 86 | }; 87 | } 88 | 89 | 90 | -------------------------------------------------------------------------------- /cartesian_trajectory_interpolation/src/cartesian_trajectory.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2021 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | 31 | //----------------------------------------------------------------------------- 32 | /*!\file cartesian_trajectory.cpp 33 | * 34 | * \author Stefan Scherzinger 35 | * \date 2021/01/21 36 | * 37 | */ 38 | //----------------------------------------------------------------------------- 39 | 40 | #include 41 | #include 42 | 43 | namespace cartesian_ros_control 44 | { 45 | 46 | void CartesianTrajectory::sample(const CartesianTrajectorySegment::Time& time, CartesianState& state) 47 | { 48 | trajectory_interface::sample(trajectory_data_, time, state); 49 | } 50 | 51 | CartesianTrajectory::CartesianTrajectory(const cartesian_control_msgs::CartesianTrajectory& ros_trajectory) 52 | { 53 | if (!init(ros_trajectory)) 54 | { 55 | throw std::invalid_argument("Trajectory not valid"); 56 | }; 57 | } 58 | 59 | bool CartesianTrajectory::init(const cartesian_control_msgs::CartesianTrajectory& ros_trajectory) 60 | { 61 | trajectory_data_.clear(); 62 | 63 | // Loop through the waypoints and build trajectory segments from each two 64 | // neighboring pairs. 65 | for (auto i = ros_trajectory.points.begin(); std::next(i) < ros_trajectory.points.end(); ++i) 66 | { 67 | // Waypoints' time from start must strictly increase 68 | if (i->time_from_start.toSec() >= std::next(i)->time_from_start.toSec()) 69 | { 70 | return false; 71 | } 72 | CartesianTrajectorySegment s( 73 | i->time_from_start.toSec(), CartesianState(*i), 74 | std::next(i)->time_from_start.toSec(), CartesianState(*std::next(i)) 75 | ); 76 | trajectory_data_.push_back(s); 77 | } 78 | return true; 79 | } 80 | } 81 | -------------------------------------------------------------------------------- /cartesian_interface/test/cartesian_state_interface_test.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2020 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | 31 | //---------------------------------------------------------------------- 32 | /*!\file 33 | * 34 | * \author Felix Exner mauch@fzi.de 35 | * \date 2020-07-02 36 | * 37 | */ 38 | //---------------------------------------------------------------------- 39 | 40 | #include 41 | 42 | #include 43 | 44 | using namespace cartesian_ros_control; 45 | 46 | TEST(CartesianStateHandleTest, TestConstructor) 47 | { 48 | std::string reference_frame = "base"; 49 | std::string controlled_frame = "tool0"; 50 | geometry_msgs::Pose pose_buffer; 51 | geometry_msgs::Twist twist_buffer; 52 | geometry_msgs::Accel accel_buffer; 53 | geometry_msgs::Accel jerk_buffer; 54 | 55 | EXPECT_NO_THROW(CartesianStateHandle obj(reference_frame, controlled_frame, &pose_buffer, &twist_buffer, 56 | &accel_buffer, &jerk_buffer)); 57 | EXPECT_THROW( 58 | CartesianStateHandle obj(reference_frame, controlled_frame, nullptr, &twist_buffer, &accel_buffer, &jerk_buffer), 59 | hardware_interface::HardwareInterfaceException); 60 | EXPECT_THROW( 61 | CartesianStateHandle obj(reference_frame, controlled_frame, &pose_buffer, nullptr, &accel_buffer, &jerk_buffer), 62 | hardware_interface::HardwareInterfaceException); 63 | EXPECT_THROW( 64 | CartesianStateHandle obj(reference_frame, controlled_frame, &pose_buffer, &twist_buffer, nullptr, &jerk_buffer), 65 | hardware_interface::HardwareInterfaceException); 66 | EXPECT_THROW( 67 | CartesianStateHandle obj(reference_frame, controlled_frame, &pose_buffer, &twist_buffer, &accel_buffer, nullptr), 68 | hardware_interface::HardwareInterfaceException); 69 | } 70 | 71 | int main(int argc, char** argv) 72 | { 73 | testing::InitGoogleTest(&argc, argv); 74 | return RUN_ALL_TESTS(); 75 | } 76 | -------------------------------------------------------------------------------- /twist_controller/include/twist_controller/twist_controller.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2020 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | 31 | //---------------------------------------------------------------------- 32 | /*!\file 33 | * 34 | * \author Felix Exner mauch@fzi.de 35 | * \date 2020-07-02 36 | * 37 | */ 38 | //---------------------------------------------------------------------- 39 | 40 | #pragma once 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | 49 | #include 50 | 51 | namespace cartesian_ros_control 52 | { 53 | /** 54 | * @brief A Cartesian ROS-controller for commanding target twists to a robot 55 | * 56 | * This controller makes use of a TwistCommandInterface to set a user specified 57 | * twist message as reference for robot control. 58 | * The according hardware_interface::RobotHW can send these commands 59 | * directly to the robot driver in its write() function. 60 | */ 61 | class TwistController : public controller_interface::Controller 62 | { 63 | public: 64 | TwistController() = default; 65 | virtual ~TwistController() = default; 66 | 67 | virtual bool init(TwistCommandInterface* hw, ros::NodeHandle& n) override; 68 | 69 | virtual void starting(const ros::Time& time) override; 70 | 71 | virtual void update(const ros::Time& /*time*/, const ros::Duration& /*period*/) override 72 | { 73 | handle_.setCommand(*command_buffer_.readFromRT()); 74 | } 75 | 76 | TwistCommandHandle handle_; 77 | realtime_tools::RealtimeBuffer command_buffer_; 78 | 79 | private: 80 | ros::Subscriber twist_sub_; 81 | void twistCallback(const geometry_msgs::TwistConstPtr& msg); 82 | void reconfigureCallback(const twist_controller::TwistControllerConfig& config, uint32_t level); 83 | double gain_; 84 | std::shared_ptr> server_; 85 | }; 86 | 87 | } // namespace cartesian_ros_control 88 | -------------------------------------------------------------------------------- /cartesian_trajectory_controller/include/inverse_kinematics/ik_solver_base.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2021 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | 31 | 32 | //----------------------------------------------------------------------------- 33 | /*!\file ik_solver_base.h 34 | * 35 | * \author Stefan Scherzinger 36 | * \date 2021/05/12 37 | * 38 | */ 39 | //----------------------------------------------------------------------------- 40 | 41 | #pragma once 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | namespace cartesian_ros_control 49 | { 50 | 51 | /** 52 | * @brief Base class for Inverse Kinematics (IK) solvers 53 | * 54 | * This base class is meant to provide an interface for custom IK implementations. 55 | * The joint-based control policies in the Cartesian trajectory 56 | * controller will need some form of IK solver. This allows you to implement 57 | * your own (possibly more advanced) algorithm. For instance, you may wish 58 | * to consider collision checking by reacting ad-hoc to objects 59 | * that additional sensors perceive in your environment. 60 | */ 61 | class IKSolver 62 | { 63 | public: 64 | IKSolver(){}; 65 | virtual ~IKSolver(){}; 66 | 67 | /** 68 | * @brief Initialize the solver 69 | * 70 | * @param robot_chain Representation of the robot kinematics 71 | * 72 | * @param root_nh A NodeHandle in the root of the controller manager namespace. 73 | * 74 | * @param controller_nh A NodeHandle in the namespace of the controller. 75 | * This is where the Cartesian trajectory controller-specific configuration resides. 76 | * 77 | * @return True if initialization was successful. 78 | */ 79 | virtual bool init(const KDL::Chain& robot_chain, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) = 0; 80 | 81 | /** 82 | * @brief Compute Inverse Kinematics 83 | * 84 | * @param q_init Vector of initial joint positions 85 | * @param goal Goal pose with respect to the robot base 86 | * @param q_out Vector of suitable joint positions 87 | * 88 | * @return 0 if successful. Derived classes implement specializations. 89 | */ 90 | virtual int cartToJnt(const KDL::JntArray& q_init, const KDL::Frame& goal, KDL::JntArray& q_out) = 0; 91 | }; 92 | 93 | } 94 | -------------------------------------------------------------------------------- /cartesian_trajectory_controller/src/cartesian_trajectory_controller.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2020 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | 31 | //----------------------------------------------------------------------------- 32 | /*!\file cartesian_trajectory_controller.cpp 33 | * 34 | * \author Stefan Scherzinger 35 | * \date 2021/01/24 36 | * 37 | */ 38 | //----------------------------------------------------------------------------- 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include "hardware_interface/joint_state_interface.h" 45 | 46 | namespace pose_controllers 47 | { 48 | using CartesianTrajectoryController = 49 | cartesian_trajectory_controller::CartesianTrajectoryController; 50 | } 51 | 52 | namespace twist_controllers 53 | { 54 | using CartesianTrajectoryController = 55 | cartesian_trajectory_controller::CartesianTrajectoryController; 56 | } 57 | 58 | namespace position_controllers 59 | { 60 | using CartesianTrajectoryController = 61 | cartesian_trajectory_controller::CartesianTrajectoryController; 62 | } 63 | 64 | namespace velocity_controllers 65 | { 66 | using CartesianTrajectoryController = 67 | cartesian_trajectory_controller::CartesianTrajectoryController; 68 | } 69 | 70 | namespace cartesian_trajectory_publisher 71 | { 72 | using CartesianTrajectoryPublisher = 73 | cartesian_trajectory_controller::CartesianTrajectoryController; 74 | } 75 | 76 | 77 | 78 | PLUGINLIB_EXPORT_CLASS(pose_controllers::CartesianTrajectoryController, 79 | controller_interface::ControllerBase) 80 | 81 | /* Not yet implemented. 82 | PLUGINLIB_EXPORT_CLASS(twist_controllers::CartesianTrajectoryController, 83 | controller_interface::ControllerBase) 84 | */ 85 | 86 | PLUGINLIB_EXPORT_CLASS(position_controllers::CartesianTrajectoryController, 87 | controller_interface::ControllerBase) 88 | /* Not yet implemented 89 | PLUGINLIB_EXPORT_CLASS(velocity_controllers::CartesianTrajectoryController, 90 | controller_interface::ControllerBase) 91 | */ 92 | 93 | PLUGINLIB_EXPORT_CLASS(cartesian_trajectory_publisher::CartesianTrajectoryPublisher, 94 | controller_interface::ControllerBase) 95 | -------------------------------------------------------------------------------- /twist_controller/src/twist_controller.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2020 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | 31 | //---------------------------------------------------------------------- 32 | /*!\file 33 | * 34 | * \author Felix Exner mauch@fzi.de 35 | * \date 2020-07-02 36 | * 37 | */ 38 | //---------------------------------------------------------------------- 39 | 40 | #include 41 | #include 42 | #include "twist_controller/TwistControllerConfig.h" 43 | 44 | namespace cartesian_ros_control 45 | { 46 | bool TwistController::init(TwistCommandInterface* hw, ros::NodeHandle& n) 47 | { 48 | std::string frame_id; 49 | if (!n.getParam("frame_id", frame_id)) 50 | { 51 | ROS_ERROR_STREAM("Required parameter " << n.resolveName("frame_id") << " not given"); 52 | return false; 53 | } 54 | 55 | gain_ = n.param("twist_gain", 0.1); 56 | server_.reset(new dynamic_reconfigure::Server(n)); 57 | server_->setCallback( 58 | boost::bind(&TwistController::reconfigureCallback, this, _1, _2)); 59 | 60 | handle_ = hw->getHandle(frame_id); 61 | twist_sub_ = n.subscribe("command", 1, &TwistController::twistCallback, this); 62 | 63 | std::vector joint_names; 64 | if (!n.getParam("joints", joint_names)) 65 | { 66 | ROS_ERROR_STREAM("Failed to read required parameter '" << n.resolveName("joints") << "."); 67 | return false; 68 | } 69 | 70 | for (auto& name : joint_names) 71 | { 72 | hw->claim(name); 73 | } 74 | 75 | return true; 76 | } 77 | 78 | void TwistController::starting(const ros::Time& time) 79 | { 80 | geometry_msgs::Twist twist; 81 | twist.linear.x = 0; 82 | twist.linear.y = 0; 83 | twist.linear.z = 0; 84 | twist.angular.x = 0; 85 | twist.angular.y = 0; 86 | twist.angular.z = 0; 87 | command_buffer_.writeFromNonRT(twist); 88 | } 89 | 90 | void TwistController::twistCallback(const geometry_msgs::TwistConstPtr& msg) 91 | { 92 | geometry_msgs::Twist twist; 93 | twist.linear.x = gain_ * msg->linear.x; 94 | twist.linear.y = gain_ * msg->linear.y; 95 | twist.linear.z = gain_ * msg->linear.z; 96 | twist.angular.x = gain_ * msg->angular.x; 97 | twist.angular.y = gain_ * msg->angular.y; 98 | twist.angular.z = gain_ * msg->angular.z; 99 | command_buffer_.writeFromNonRT(twist); 100 | } 101 | 102 | void TwistController ::reconfigureCallback(const twist_controller::TwistControllerConfig& config, uint32_t level) 103 | { 104 | gain_ = config.twist_gain; 105 | } 106 | } // namespace cartesian_ros_control 107 | 108 | PLUGINLIB_EXPORT_CLASS(cartesian_ros_control::TwistController, controller_interface::ControllerBase) 109 | -------------------------------------------------------------------------------- /cartesian_interface/README.md: -------------------------------------------------------------------------------- 1 | # Cartesian Interface 2 | 3 | This package provides new hardware interfaces to implement Cartesian ROS control 4 | for robot manipulators. 5 | 6 | ## Rationale 7 | Several OEMs provide native Cartesian interfaces in their robot drivers. 8 | By offering new according hardware interfaces, implementers of ROS controllers get direct access to Cartesian buffers for reading and writing. 9 | Whether this provides advantages over the classic joint-based interfaces of course depends on personal use cases. 10 | The main difference is that in the Cartesian case the robot takes care of Inverse Kinematics. 11 | 12 | ## Usage 13 | This package provides ``PoseCommandInterface`` and ``TwistCommandInterface`` 14 | for read/write access and ``CartesianStateInterface`` for pure read access. 15 | You would add them to your hardware abstraction like this: 16 | 17 | ```c++ 18 | #include 19 | #include 20 | 21 | class YourRobot : public hardware_interface::RobotHW 22 | { 23 | ... 24 | 25 | // New interfaces for Cartesian ROS-controllers 26 | cartesian_ros_control::CartesianStateInterface cart_interface_; 27 | cartesian_ros_control::TwistCommandInterface twist_interface_; 28 | cartesian_ros_control::PoseCommandInterface pose_interface_; 29 | ... 30 | 31 | // Buffers for read/write access 32 | geometry_msgs::Pose cart_pose_; 33 | geometry_msgs::Twist cart_twist_; 34 | geometry_msgs::Accel cart_accel_; 35 | geometry_msgs::Accel cart_jerk_; 36 | geometry_msgs::Twist twist_command_; 37 | geometry_msgs::Pose pose_command_; 38 | ... 39 | 40 | } 41 | ``` 42 | Registering them could look like this: 43 | ```c++ 44 | YourRobot::YourRobot() 45 | { 46 | ... 47 | cartesian_ros_control::CartesianStateHandle cart_state_handle("base", "tip", &cart_pose_, &cart_twist_, 48 | &cart_accel_, &cart_jerk_); 49 | cart_interface_.registerHandle(cart_state_handle); 50 | 51 | twist_interface_.registerHandle( 52 | cartesian_ros_control::TwistCommandHandle(cart_interface_.getHandle("tip"), &twist_command_)); 53 | twist_interface_.getHandle("tip"); 54 | 55 | pose_interface_.registerHandle( 56 | cartesian_ros_control::PoseCommandHandle(cart_interface_.getHandle("tip"), &pose_command_)); 57 | pose_interface_.getHandle("tip"); 58 | 59 | // Register interfaces 60 | registerInterface(&twist_interface_); 61 | registerInterface(&pose_interface_); 62 | ... 63 | } 64 | 65 | ``` 66 | 67 | Note the two strings `base` and `tip` during instantiation of the Cartesian state handle. 68 | They represent frames in your robot kinematics and their names will vary from robot to robot. 69 | There's a convention behind that assumes that `base` is the reference frame in which `tip` is given. 70 | `tip` is the end-point of the robot we want to control and represents a unique identifier for the handle. 71 | You can think of it as resource names in joint-based ROS control. 72 | 73 | 74 | ## Cartesian ROS controllers 75 | 76 | When implementing a ROS controller, you would add the according handle for 77 | read/write access, e.g. in the case of Cartesian pose control: 78 | ```c++ 79 | #include 80 | ... 81 | 82 | class YourController : public controller_interface::Controller 83 | { 84 | 85 | ... 86 | private: 87 | cartesian_ros_control::PoseCommandHandle handle_; 88 | ... 89 | } 90 | ``` 91 | During instantiation of the handle, you ask for the end-point you want to control: 92 | ```c++ 93 | handle_ = hw->getHandle("tip"); 94 | ``` 95 | and then use that handle to implement your control loop in `update(...)`: 96 | ```c++ 97 | 98 | // Get current state from the robot hardware 99 | geometry_msgs::Pose pose = handle_.getPose(); 100 | geometry_msgs::Twist twist = handle_.getTwist(); 101 | geometry_msgs::Accel accel = handle_.getAccel(); 102 | 103 | geometry_msgs::Pose target; 104 | 105 | // Implement your control law here 106 | 107 | handle_.setCommand(target); 108 | ... 109 | ``` 110 | 111 | *** 112 | 116 | 117 | 118 | rosin_logo 120 | 121 | 122 | Supported by ROSIN - ROS-Industrial Quality-Assured Robot Software Components. 123 | More information: rosin-project.eu 124 | 125 | eu_flag 127 | 128 | This project has received funding from the European Union’s Horizon 2020 129 | research and innovation programme under grant agreement no. 732287. 130 | -------------------------------------------------------------------------------- /cartesian_trajectory_interpolation/include/cartesian_trajectory_interpolation/cartesian_state.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2021 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | 31 | #pragma once 32 | 33 | // Eigen 34 | #include 35 | #include 36 | #include "Eigen/src/Core/Matrix.h" 37 | #include "Eigen/src/Geometry/AngleAxis.h" 38 | #include "Eigen/src/Geometry/Quaternion.h" 39 | 40 | #include 41 | 42 | namespace cartesian_ros_control 43 | { 44 | 45 | /** 46 | * @brief Cartesian state with pose, velocity and acceleration 47 | * 48 | * All quantities are assumed to be given in one common reference frame. 49 | * This frame is also the reference for the pose defined by \ref p and \ref q. 50 | * 51 | */ 52 | struct CartesianState 53 | { 54 | 55 | /** 56 | * @brief Initializes all quantities to zero and sets the orientation quaternion to identity 57 | */ 58 | CartesianState(); 59 | 60 | /** 61 | * @brief Convenience constructor for ROS messages 62 | * 63 | * Implicitly normalizes the point's orientation quaternion. 64 | * 65 | * @param point The desired state 66 | */ 67 | CartesianState(const cartesian_control_msgs::CartesianTrajectoryPoint& point); 68 | 69 | /** 70 | * @brief Difference operator between states 71 | * 72 | * This is the element-wise difference for all vector quantities and the 73 | * difference of rotation for the quaternion. 74 | * 75 | * @param other State to subtract 76 | * 77 | * @return The element-wise difference of the two. 78 | */ 79 | CartesianState operator-(const CartesianState& other) const; 80 | 81 | /** 82 | * @brief Convenience method for conversion 83 | * 84 | * @param time_from_start Time from start in seconds 85 | * 86 | * @return Cartesian State in trajectory waypoint representation 87 | */ 88 | cartesian_control_msgs::CartesianTrajectoryPoint toMsg(int time_from_start = 0) const; 89 | 90 | /** 91 | * @brief Get Euler-Rodrigues vector from orientation 92 | * 93 | * @return The orientation in axis-angle notation 94 | */ 95 | Eigen::Vector3d rot() const {Eigen::AngleAxisd a(q); return a.axis() * a.angle();}; 96 | 97 | /** 98 | * @brief Stream operator for testing and debugging 99 | * 100 | * @param os The output stream 101 | * @param state The CartesianState 102 | * 103 | * @return Reference to the stream for chaining 104 | */ 105 | friend std::ostream& operator<<(std::ostream &os, const CartesianState& state); 106 | 107 | // Pose 108 | Eigen::Vector3d p; ///< position 109 | Eigen::Quaterniond q; ///< rotation 110 | 111 | // Spatial velocity in body (waypoint) coordinates 112 | Eigen::Vector3d v; ///< linear velocity, \f$ v \f$ 113 | Eigen::Vector3d w; ///< angular velocity, \f$ \omega \f$ 114 | 115 | // Spatial acceleration in body (waypoint) coordinates 116 | Eigen::Vector3d v_dot; ///< linear acceleration, \f$ \dot{v} \f$ 117 | Eigen::Vector3d w_dot; ///< angular acceleration, \f$ \dot{\omega} \f$ 118 | 119 | }; 120 | 121 | } 122 | -------------------------------------------------------------------------------- /cartesian_trajectory_controller/include/cartesian_trajectory_controller/cartesian_trajectory_controller.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2020 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | 31 | //----------------------------------------------------------------------------- 32 | /*!\file cartesian_trajectory_controller.h 33 | * 34 | * \author Stefan Scherzinger 35 | * \date 2021/01/24 36 | * 37 | */ 38 | //----------------------------------------------------------------------------- 39 | 40 | #pragma once 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | namespace cartesian_trajectory_controller 53 | { 54 | 55 | template 56 | class CartesianTrajectoryController : public cartesian_ros_control::ControlPolicy 57 | { 58 | 59 | public: 60 | CartesianTrajectoryController() 61 | : cartesian_ros_control::ControlPolicy() 62 | {}; 63 | 64 | virtual ~CartesianTrajectoryController(){}; 65 | 66 | bool init(hardware_interface::RobotHW* hw, 67 | ros::NodeHandle& root_nh, 68 | ros::NodeHandle& controller_nh) override; 69 | 70 | void starting(const ros::Time& time) override; 71 | 72 | void stopping(const ros::Time& time) override; 73 | 74 | void update(const ros::Time& time, const ros::Duration& period) override; 75 | 76 | void executeCB(const cartesian_control_msgs::FollowCartesianTrajectoryGoalConstPtr& goal); 77 | 78 | void preemptCB(); 79 | 80 | protected: 81 | using ControlPolicy = cartesian_ros_control::ControlPolicy; 82 | 83 | struct TrajectoryDuration 84 | { 85 | TrajectoryDuration() : now(0.0), end(0.0) {} 86 | 87 | ros::Duration end; ///< Planned target duration of the current action. 88 | ros::Duration now; ///< Current duration of the current action. 89 | }; 90 | 91 | void timesUp(); 92 | 93 | void monitorExecution(const cartesian_ros_control::CartesianState& error); 94 | 95 | bool withinTolerances(const cartesian_ros_control::CartesianState& error, 96 | const cartesian_control_msgs::CartesianTolerance& tolerance); 97 | 98 | private: 99 | std::unique_ptr speed_scaling_; 100 | std::unique_ptr > 101 | action_server_; 102 | std::atomic done_; 103 | std::mutex lock_; 104 | cartesian_ros_control::CartesianTrajectory trajectory_; 105 | TrajectoryDuration trajectory_duration_; 106 | cartesian_control_msgs::CartesianTolerance path_tolerances_; 107 | cartesian_control_msgs::CartesianTolerance goal_tolerances_; 108 | }; 109 | 110 | } 111 | 112 | #include 113 | -------------------------------------------------------------------------------- /cartesian_trajectory_interpolation/src/cartesian_state.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2021 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | 31 | //----------------------------------------------------------------------------- 32 | /*!\file cartesian_state.cpp 33 | * 34 | * \author Stefan Scherzinger 35 | * \date 2021/01/25 36 | * 37 | */ 38 | //----------------------------------------------------------------------------- 39 | 40 | #include 41 | #include 42 | #include "Eigen/src/Core/Matrix.h" 43 | #include "geometry_msgs/Vector3.h" 44 | 45 | 46 | namespace my_tf2 47 | { 48 | geometry_msgs::Vector3 toMsg(const Eigen::Vector3d& in) 49 | { 50 | geometry_msgs::Vector3 msg; 51 | msg.x = in.x(); 52 | msg.y = in.y(); 53 | msg.z = in.z(); 54 | return msg; 55 | }; 56 | } 57 | 58 | 59 | namespace cartesian_ros_control 60 | { 61 | 62 | CartesianState::CartesianState() 63 | { 64 | p = Eigen::Vector3d::Zero(); 65 | q.x() = 0; 66 | q.y() = 0; 67 | q.z() = 0; 68 | q.w() = 1; 69 | 70 | v = Eigen::Vector3d::Zero(); 71 | v_dot = Eigen::Vector3d::Zero(); 72 | 73 | w = Eigen::Vector3d::Zero(); 74 | w_dot = Eigen::Vector3d::Zero(); 75 | } 76 | 77 | 78 | CartesianState::CartesianState(const cartesian_control_msgs::CartesianTrajectoryPoint& point) 79 | { 80 | // Pose 81 | tf2::fromMsg(point.pose.position, p); 82 | tf2::fromMsg(point.pose.orientation, q); 83 | if (q.coeffs() == Eigen::Vector4d::Zero()) 84 | { 85 | q.w() = 1; 86 | } 87 | q.normalize(); 88 | 89 | // Velocity 90 | tf2::fromMsg(point.twist.linear, v); 91 | tf2::fromMsg(point.twist.angular, w); 92 | 93 | // Acceleration 94 | tf2::fromMsg(point.acceleration.linear, v_dot); 95 | tf2::fromMsg(point.acceleration.angular, w_dot); 96 | } 97 | 98 | CartesianState CartesianState::operator-(const CartesianState& other) const 99 | { 100 | CartesianState result; 101 | result.p = p - other.p; 102 | result.q = q * other.q.inverse(); 103 | result.v = v - other.v; 104 | result.w = w - other.w; 105 | result.v_dot = v_dot - other.v_dot; 106 | result.w_dot = w_dot - other.w_dot; 107 | 108 | return result; 109 | } 110 | 111 | cartesian_control_msgs::CartesianTrajectoryPoint CartesianState::toMsg(int time_from_start) const 112 | { 113 | cartesian_control_msgs::CartesianTrajectoryPoint point; 114 | 115 | // Pose 116 | point.pose.position = tf2::toMsg(p); 117 | point.pose.orientation = tf2::toMsg(q); 118 | 119 | // Velocity 120 | point.twist.linear = my_tf2::toMsg(v); 121 | point.twist.angular = my_tf2::toMsg(w); 122 | 123 | // Acceleration 124 | point.acceleration.linear = my_tf2::toMsg(v_dot); 125 | point.acceleration.angular = my_tf2::toMsg(w_dot); 126 | 127 | return point; 128 | } 129 | 130 | std::ostream& operator<<(std::ostream &out, const CartesianState& state) 131 | { 132 | out << "p:\n" << state.p << '\n'; 133 | out << "q:\n" << state.q.coeffs() << '\n'; 134 | out << "v:\n" << state.v << '\n'; 135 | out << "w:\n" << state.w << '\n'; 136 | out << "v_dot:\n" << state.v_dot << '\n'; 137 | out << "w_dot:\n" << state.w_dot; 138 | return out; 139 | } 140 | } 141 | -------------------------------------------------------------------------------- /cartesian_trajectory_interpolation/include/cartesian_trajectory_interpolation/cartesian_trajectory.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2021 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | 31 | 32 | //----------------------------------------------------------------------------- 33 | /*!\file cartesian_trajectory.h 34 | * 35 | * \author Stefan Scherzinger 36 | * \date 2021/01/14 37 | * 38 | */ 39 | //----------------------------------------------------------------------------- 40 | 41 | #pragma once 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | 49 | namespace cartesian_ros_control 50 | { 51 | 52 | /** 53 | * @brief A class for Cartesian trajectory representation and interpolation 54 | * 55 | * It's meant to be used inside ROS controllers to wrap the complexity of 56 | * trajectory interpolation. Initialize instances of this helper with 57 | * Cartesian ROS trajectories and sample \a CartesianState at specific time 58 | * steps for robot control. 59 | * 60 | */ 61 | class CartesianTrajectory 62 | { 63 | public: 64 | CartesianTrajectory() = default; 65 | 66 | /** 67 | * @brief Construct from ROS messages 68 | * 69 | * Calls init() and throws std::invalid_argument if that fails. 70 | * 71 | * @param ros_trajectory The Cartesian trajectory composed with ROS message types 72 | */ 73 | CartesianTrajectory(const cartesian_control_msgs::CartesianTrajectory& ros_trajectory); 74 | 75 | virtual ~CartesianTrajectory(){}; 76 | 77 | /** 78 | * @brief Initialize from ROS message 79 | * 80 | * \note The first waypoint is expected to be the current state with time_from_start == 0. 81 | * 82 | * @param ros_trajectory The Cartesian trajectory composed with ROS message types 83 | * 84 | * @return True if \b ros_trajectory has increasing waypoints in time, else false. 85 | */ 86 | bool init(const cartesian_control_msgs::CartesianTrajectory& ros_trajectory); 87 | 88 | /** 89 | * @brief Sample a trajectory at a specified time 90 | * 91 | * The trajectory's waypoints are interpolated with splines (two-point 92 | * polynomials) that are uniquely defined between each two waypoints and 93 | * that scale with the fields given: 94 | * 95 | * - Only pose = linear interpolation 96 | * - Pose and velocity = cubic interpolation 97 | * - Pose, velocity and acceleration = quintic interpolation 98 | * 99 | * If this trajectory's waypoints have velocity and acceleration 100 | * setpoints, \b state also contains the current velocity and 101 | * acceleration, respectively. A typical application is using the 102 | * sampled state as reference for robot control. 103 | * 104 | * @param time Time at which to sample the trajectory 105 | * @param state Cartesian state at \b time 106 | */ 107 | void sample(const CartesianTrajectorySegment::Time& time, CartesianState& state); 108 | 109 | 110 | private: 111 | std::vector trajectory_data_; 112 | }; 113 | 114 | } 115 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![ROS Industrial 2 | CI](https://github.com/fzi-forschungszentrum-informatik/cartesian_ros_control/actions/workflows/industrial_ci_action.yml/badge.svg?branch=master&event=push)](https://github.com/fzi-forschungszentrum-informatik/cartesian_ros_control/actions) 3 | 4 | --- 5 | ## ++ Deprecation Notice ++ 6 | 7 | This repository has been split and restructured into individual packages to 8 | facilitate upstream merges into ROS mainline repositories. 9 | The development has been moved to the following places: 10 | - Message definition for Cartesian trajectories: [cartesian_control_msgs](https://github.com/UniversalRobots/Universal_Robots_ROS_cartesian_control_msgs) 11 | - Interface definitions and controllers: [ros_controllers_cartesian](https://github.com/UniversalRobots/Universal_Robots_ROS_controllers_cartesian) 12 | - Speed scaling interface: [scaled_controllers](https://github.com/UniversalRobots/Universal_Robots_ROS_scaled_controllers) 13 | - Trajectory forwarding: [pass_through_controllers](https://github.com/UniversalRobots/Universal_Robots_ROS_passthrough_controllers) 14 | 15 | Please continue to file bugs and open pull requests there as usual. 16 | 17 | --- 18 | # Cartesian ROS Control 19 | 20 | This package brings mechanisms for Cartesian control to the ROS-control framework. 21 | 22 | ## Overview 23 | 24 | New functionality (colored): 25 | 26 | ![Colored: New contributions from this package](./cartesian_ros_control/doc/cartesian_ros_control.png) 27 | 28 | 29 | ## Rationale 30 | 31 | As opposed to joint-based control, Cartesian control is often more intuitive for programmers to specify how a tool (robot end-effector) should move in their application. 32 | For instance, gluing, grinding, polishing and all sorts of other surface-related tasks benefit from a straight-forward task formulation with Cartesian coordinates. 33 | 34 | It comes at a surprise that there hasn't been native support of Cartesian control in ROS. Yet, the number of OEMs, whose drivers support Cartesian control interfaces is growing. 35 | This set of packages aims at filling this gap and get you started with Cartesian control. 36 | 37 | 38 | ## Major features at a glance 39 | - **Add Cartesian functionality to ROS control**. This brings new interfaces for 40 | controller design, such as a ```PoseCommandInterface```, a ```TwistCommandInterface```, and a new Cartesian trajectory definition. Example controllers include a ```TwistController``` and a ```CartesianTrajectoryController```. 41 | 42 | - **Enable Cartesian trajectory control** in your applications. Specify your task comfortably with 43 | waypoints in task space. ROS-side interpolation and streaming of setpoints over the new interfaces is only one of several alternatives. 44 | 45 | - **Use (conventional) ROS control** for Cartesian trajectory execution. You don't need to change anything in the driver's HW-abstraction of your specific robot if that supports current ROS control. 46 | 47 | - **Hand-over control to the robot by forwarding trajectories**. 48 | Two new interfaces ```CartesianTrajectoryInterface``` and ```JointTrajectoryInterface``` let robots take care of driver-side interpolation to achieve best performance. 49 | 50 | - **Speed-scale trajectory execution**. All trajectory executions (both Cartesian and joint-based) can be speed-scaled within 0 to 100% at runtime. This gives you flexibility in setting-up new applications and during test runs. Changing this continuously even lets you reshape trajectory execution without re-teaching. 51 | 52 | ## Robots Overview 53 | In the spirit of ROS control, the implementation is robot-agnostic and shall support applications on a wide range of robots. The table below shows what features will be available with this enhancement. 54 | 55 | | Feature | Robots with new interfaces | Robots with current ROS control | 56 | | -------- | -------- | --- | 57 | | Cartesian trajectory control | ✓ | ✓ | 58 | | Cartesian trajectory forwarding | ✓ | | 59 | | Joint trajectory forwarding | ✓ | | 60 | | Speed-scale trajectories | ✓ | | 61 | 62 | 63 | ## Acknowledgement 64 | Developed in collaboration between: 65 | 66 | [Universal Robots A/S](https://www.universal-robots.com/)   and   67 | [FZI Research Center for Information Technology](https://www.fzi.de). 68 | 69 | *** 70 | 74 | 75 | 76 | rosin_logo 78 | 79 | 80 | Supported by ROSIN - ROS-Industrial Quality-Assured Robot Software Components. 81 | More information: rosin-project.eu 82 | 83 | eu_flag 85 | 86 | This project has received funding from the European Union’s Horizon 2020 87 | research and innovation programme under grant agreement no. 732287. 88 | -------------------------------------------------------------------------------- /pass_through_controllers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pass_through_controllers 4 | 0.0.0 5 | 6 | Trajectory controllers (joint-based and Cartesian) that forward trajectories 7 | directly to a robot controller and let it handle trajectory interpolation and execution. 8 | 9 | 10 | 11 | 12 | scherzin 13 | 14 | 15 | 16 | 17 | 18 | BSD 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | catkin 54 | 55 | roscpp 56 | trajectory_msgs 57 | cartesian_control_msgs 58 | geometry_msgs 59 | cartesian_interface 60 | controller_manager 61 | hardware_interface 62 | actionlib 63 | dynamic_reconfigure 64 | 65 | roscpp 66 | trajectory_msgs 67 | cartesian_control_msgs 68 | geometry_msgs 69 | cartesian_interface 70 | controller_manager 71 | hardware_interface 72 | actionlib 73 | dynamic_reconfigure 74 | 75 | roscpp 76 | trajectory_msgs 77 | cartesian_control_msgs 78 | geometry_msgs 79 | cartesian_interface 80 | controller_manager 81 | hardware_interface 82 | actionlib 83 | dynamic_reconfigure 84 | 85 | 86 | cartesian_trajectory_controller 87 | actionlib 88 | actionlib_msgs 89 | control_msgs 90 | controller_manager_msgs 91 | rostest 92 | xacro 93 | 94 | 95 | controller_interface 96 | 97 | 98 | 99 | 100 | 101 | 105 | 106 | 107 | 108 | -------------------------------------------------------------------------------- /cartesian_trajectory_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cartesian_trajectory_controller 4 | 0.0.0 5 | A Cartesian trajectory controller with multiple hardware interface support 6 | 7 | 8 | 9 | 10 | scherzin 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | roscpp 54 | rospy 55 | cartesian_interface 56 | controller_manager 57 | hardware_interface 58 | kdl_parser 59 | pluginlib 60 | cartesian_trajectory_interpolation 61 | 62 | roscpp 63 | rospy 64 | cartesian_interface 65 | controller_manager 66 | hardware_interface 67 | kdl_parser 68 | pluginlib 69 | cartesian_trajectory_interpolation 70 | 71 | roscpp 72 | rospy 73 | cartesian_interface 74 | controller_manager 75 | hardware_interface 76 | 77 | actionlib 78 | actionlib_msgs 79 | control_msgs 80 | controller_manager_msgs 81 | joint_state_controller 82 | joint_trajectory_controller 83 | robot_state_publisher 84 | ros_control_boilerplate 85 | rostest 86 | trajectory_msgs 87 | xacro 88 | 89 | 90 | controller_interface 91 | 92 | 93 | 94 | 95 | 96 | 100 | 101 | 102 | 107 | 108 | 109 | 110 | -------------------------------------------------------------------------------- /pass_through_controllers/test/test_forward_cartesian_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import unittest 3 | import copy 4 | import rospy 5 | import actionlib 6 | import sys 7 | from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal 8 | from cartesian_control_msgs.msg import ( 9 | FollowCartesianTrajectoryAction, 10 | FollowCartesianTrajectoryGoal, 11 | FollowCartesianTrajectoryResult, 12 | CartesianTrajectoryPoint) 13 | from trajectory_msgs.msg import JointTrajectoryPoint 14 | from controller_manager_msgs.srv import SwitchControllerRequest, SwitchController 15 | 16 | PKG = 'pass_through_controllers' 17 | NAME = 'test_cartesian_trajectory_forwarding' 18 | 19 | 20 | class IntegrationTest(unittest.TestCase): 21 | 22 | def __init__(self, *args): 23 | super(IntegrationTest, self).__init__(*args) 24 | 25 | rospy.init_node(NAME) 26 | 27 | timeout = rospy.Duration(3) 28 | 29 | self.set_joints = actionlib.SimpleActionClient( 30 | "/robot/joint_trajectory_controller/follow_joint_trajectory", FollowJointTrajectoryAction) 31 | if not self.set_joints.wait_for_server(timeout): 32 | self.fail("Could not reach robot action.") 33 | 34 | self.client = actionlib.SimpleActionClient( 35 | '/hw_interface/forward_cartesian_trajectories/follow_cartesian_trajectory', 36 | FollowCartesianTrajectoryAction) 37 | if not self.client.wait_for_server(timeout): 38 | self.fail("Could not reach hw_interface action.") 39 | 40 | self.switch_robot_ctrl = rospy.ServiceProxy('/robot/controller_manager/switch_controller', SwitchController) 41 | try: 42 | self.switch_robot_ctrl.wait_for_service(timeout.to_sec()) 43 | except rospy.exceptions.ROSException as err: 44 | self.fail( 45 | "Could not reach robot controller switch service. Msg: {}".format(err)) 46 | 47 | self.switch_forward_ctrl = rospy.ServiceProxy('/hw_interface/controller_manager/switch_controller', SwitchController) 48 | try: 49 | self.switch_forward_ctrl.wait_for_service(timeout.to_sec()) 50 | except rospy.exceptions.ROSException as err: 51 | self.fail( 52 | "Could not reach hw_interface controller switch service. Msg: {}".format(err)) 53 | 54 | def test_normal_execution(self): 55 | """ Test the basic functionality by moving on a straight line """ 56 | self.move_to_start() 57 | self.switch_to_cartesian_control() 58 | self.move() 59 | self.assertEqual(self.client.get_result().error_code, 60 | FollowCartesianTrajectoryResult.SUCCESSFUL) 61 | 62 | def move_to_start(self): 63 | srv = SwitchControllerRequest() 64 | srv.stop_controllers = ['cartesian_trajectory_controller'] 65 | srv.start_controllers = ['joint_trajectory_controller'] 66 | srv.strictness = SwitchControllerRequest.BEST_EFFORT 67 | self.switch_robot_ctrl(srv) 68 | 69 | q_start = [0, -2.0, 2.26, -0.2513274122872, 1.57, 0.0] # From base to tip 70 | start_joint_state = FollowJointTrajectoryGoal() 71 | start_joint_state.trajectory.joint_names = [ 72 | 'joint1', 73 | 'joint2', 74 | 'joint3', 75 | 'joint4', 76 | 'joint5', 77 | 'joint6'] 78 | 79 | start_joint_state.trajectory.points = [ 80 | JointTrajectoryPoint(positions=q_start, time_from_start=rospy.Duration(3.0))] 81 | self.set_joints.send_goal( 82 | start_joint_state) 83 | self.set_joints.wait_for_result() 84 | 85 | def switch_to_cartesian_control(self): 86 | """ Assume possibly running joint controllers""" 87 | 88 | # Prepare robot dummy 89 | srv = SwitchControllerRequest() 90 | srv.stop_controllers = ['joint_trajectory_controller'] 91 | srv.start_controllers = ['cartesian_trajectory_controller'] 92 | srv.strictness = SwitchControllerRequest.BEST_EFFORT 93 | self.switch_robot_ctrl(srv) 94 | 95 | # Prepare passthrough controller 96 | srv = SwitchControllerRequest() 97 | srv.stop_controllers = ['forward_joint_trajectories'] 98 | srv.start_controllers = ['forward_cartesian_trajectories'] 99 | srv.strictness = SwitchControllerRequest.BEST_EFFORT 100 | self.switch_forward_ctrl(srv) 101 | 102 | def move(self): 103 | """ Move on a straight line """ 104 | 105 | # Cartesian end-effector pose that corresponds to the start joint 106 | # configuration 107 | start = CartesianTrajectoryPoint() 108 | start.pose.position.x = 0.354 109 | start.pose.position.y = 0.180 110 | start.pose.position.z = 0.390 111 | start.pose.orientation.x = 0.502 112 | start.pose.orientation.y = 0.502 113 | start.pose.orientation.z = 0.498 114 | start.pose.orientation.w = 0.498 115 | 116 | duration = 5 117 | 118 | p1 = copy.deepcopy(start) 119 | p1.pose.position.z += 0.3 120 | p1.time_from_start = rospy.Duration(duration) 121 | 122 | goal = FollowCartesianTrajectoryGoal() 123 | goal.trajectory.points.append(p1) 124 | 125 | self.client.send_goal(goal) 126 | self.client.wait_for_result() 127 | 128 | 129 | if __name__ == '__main__': 130 | import rostest 131 | rostest.run(PKG, NAME, IntegrationTest, sys.argv) 132 | -------------------------------------------------------------------------------- /cartesian_interface/include/cartesian_interface/cartesian_command_interface.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2020 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | 31 | //---------------------------------------------------------------------- 32 | /*!\file 33 | * 34 | * \author Felix Exner mauch@fzi.de 35 | * \date 2020-07-02 36 | * 37 | */ 38 | //---------------------------------------------------------------------- 39 | 40 | #pragma once 41 | 42 | #include 43 | 44 | namespace cartesian_ros_control 45 | { 46 | 47 | /** 48 | * @brief A handle for setting pose commands 49 | * 50 | * Cartesian ROS-controllers can use this handle to write their control signals 51 | * to the according PoseCommandInterface. 52 | */ 53 | class PoseCommandHandle : public CartesianStateHandle 54 | { 55 | public: 56 | PoseCommandHandle() = default; 57 | PoseCommandHandle(const CartesianStateHandle& state_handle, geometry_msgs::Pose* cmd) 58 | : CartesianStateHandle(state_handle), cmd_(cmd) 59 | { 60 | if (!cmd) 61 | { 62 | throw hardware_interface::HardwareInterfaceException("Cannot create pose command handle for frame '" + 63 | state_handle.getName() + "'. Command data pointer is null."); 64 | } 65 | } 66 | virtual ~PoseCommandHandle() = default; 67 | 68 | void setCommand(const geometry_msgs::Pose& pose) 69 | { 70 | assert(cmd_); 71 | *cmd_ = pose; 72 | } 73 | 74 | geometry_msgs::Pose getCommand() const 75 | { 76 | assert(cmd_); 77 | return *cmd_; 78 | } 79 | const geometry_msgs::Pose* getCommandPtr() const 80 | { 81 | assert(cmd_); 82 | return cmd_; 83 | } 84 | 85 | private: 86 | geometry_msgs::Pose* cmd_ = { nullptr }; 87 | }; 88 | 89 | /** 90 | * @brief A handle for setting twist commands 91 | * 92 | * Cartesian ROS-controllers can use this handle to write their control signals 93 | * to the according TwistCommandInterface. 94 | */ 95 | class TwistCommandHandle : public CartesianStateHandle 96 | { 97 | public: 98 | TwistCommandHandle() = default; 99 | TwistCommandHandle(const CartesianStateHandle& state_handle, geometry_msgs::Twist* cmd) 100 | : CartesianStateHandle(state_handle), cmd_(cmd) 101 | { 102 | if (!cmd) 103 | { 104 | throw hardware_interface::HardwareInterfaceException("Cannot create twist command handle for frame '" + 105 | state_handle.getName() + "'. Command data pointer is null."); 106 | } 107 | } 108 | virtual ~TwistCommandHandle() = default; 109 | 110 | void setCommand(const geometry_msgs::Twist& twist) 111 | { 112 | assert(cmd_); 113 | *cmd_ = twist; 114 | } 115 | 116 | geometry_msgs::Twist getCommand() const 117 | { 118 | assert(cmd_); 119 | return *cmd_; 120 | } 121 | const geometry_msgs::Twist* getCommandPtr() const 122 | { 123 | assert(cmd_); 124 | return cmd_; 125 | } 126 | 127 | private: 128 | geometry_msgs::Twist* cmd_ = { nullptr }; 129 | }; 130 | 131 | /** 132 | * @brief A Cartesian command interface for poses 133 | * 134 | * Use an instance of this class to provide Cartesian ROS-controllers with 135 | * mechanisms to set poses as commands in the hardware_interface::RobotHW 136 | * abstraction. 137 | */ 138 | class PoseCommandInterface 139 | : public hardware_interface::HardwareResourceManager 140 | { 141 | }; 142 | 143 | /** 144 | * @brief A Cartesian command interface for twists 145 | * 146 | * Use an instance of this class to provide Cartesian ROS-controllers with 147 | * mechanisms to set twists as commands in the hardware_interface::RobotHW 148 | * abstraction. 149 | */ 150 | class TwistCommandInterface 151 | : public hardware_interface::HardwareResourceManager 152 | { 153 | }; 154 | } // namespace cartesian_ros_control 155 | -------------------------------------------------------------------------------- /cartesian_trajectory_interpolation/test/cartesian_state_test.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2021 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | 31 | 32 | //----------------------------------------------------------------------------- 33 | /*!\file cartesian_state_test.cpp 34 | * 35 | * \author Stefan Scherzinger 36 | * \date 2021/05/20 37 | * 38 | */ 39 | //----------------------------------------------------------------------------- 40 | 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | using namespace cartesian_ros_control; 49 | 50 | TEST(TestCartesianState, EmptyStateIsZeroInitialized) // except for quaternion w 51 | { 52 | CartesianState state; 53 | for (size_t i = 0; i < 3; ++i) 54 | { 55 | EXPECT_EQ(state.p[i], 0); 56 | EXPECT_EQ(state.v[i], 0); 57 | EXPECT_EQ(state.v_dot[i], 0); 58 | EXPECT_EQ(state.w[i], 0); 59 | EXPECT_EQ(state.w_dot[i], 0); 60 | } 61 | } 62 | 63 | TEST(TestCartesianState, EmptyStateHasNormalizedQuaternion) 64 | { 65 | CartesianState state; 66 | Eigen::Quaterniond q; 67 | q.w() = 1; 68 | q.normalize(); 69 | EXPECT_DOUBLE_EQ(q.norm(), state.q.norm()); 70 | } 71 | 72 | TEST(TestCartesianState, EmptyStateYieldsZeroRotationVector) 73 | { 74 | CartesianState state; 75 | EXPECT_EQ(Eigen::Vector3d::Zero(), state.rot()); 76 | } 77 | 78 | TEST(TestCartesianState, EmptyStateYieldsNormalizedTrajectoryPoint) 79 | { 80 | CartesianState state; 81 | cartesian_control_msgs::CartesianTrajectoryPoint point; 82 | point.pose.orientation.w = 1; 83 | 84 | // Compact check if both `read` the same. 85 | std::stringstream state_str; 86 | std::stringstream point_str; 87 | state_str << state.toMsg(); 88 | point_str << point; 89 | 90 | EXPECT_EQ(point_str.str(), state_str.str()); 91 | } 92 | 93 | TEST(TestCartesianState, RosMessageInitializationYieldsNormalizedQuaternions) 94 | { 95 | cartesian_control_msgs::CartesianTrajectoryPoint init; 96 | auto c = CartesianState(init); 97 | EXPECT_DOUBLE_EQ(1.0, c.q.norm()); 98 | } 99 | 100 | TEST(TestCartesianState, ConversionReturnsInitializingArgument) 101 | { 102 | // Fill some fields with arbitrary values. 103 | // Note that jerk, posture and time_from_start do not have a representation 104 | // in CartesianState and are therefore initialized to 0 (by default) in order to make their string representations to be expected the same.. 105 | 106 | cartesian_control_msgs::CartesianTrajectoryPoint init; 107 | init.acceleration.angular.x = 1.2345; 108 | init.acceleration.linear.y = -173.47; 109 | init.pose.orientation.w = 1; 110 | init.pose.position.z = 0.003; 111 | init.twist.linear.x = 67.594; 112 | 113 | CartesianState state(init); 114 | std::stringstream init_str; 115 | std::stringstream state_str; 116 | init_str << init; 117 | state_str << state.toMsg(); 118 | 119 | EXPECT_EQ(init_str.str(), state_str.str()); 120 | } 121 | 122 | TEST(CartesianState, RotationDifferenceIsPlausible) 123 | { 124 | CartesianState a; 125 | CartesianState b; 126 | b.p[0] = 1; 127 | b.v[0] = 1; 128 | b.w[0] = 1; 129 | b.v_dot[0] = 1; 130 | b.w_dot[0] = 1; 131 | 132 | // Subtraction from zero yields the negative argument 133 | auto diff = a - b; 134 | EXPECT_DOUBLE_EQ(-b.p[0], diff.p[0]); 135 | EXPECT_DOUBLE_EQ(-b.v[0], diff.v[0]); 136 | EXPECT_DOUBLE_EQ(-b.w[0], diff.w[0]); 137 | EXPECT_DOUBLE_EQ(-b.v_dot[0], diff.v_dot[0]); 138 | EXPECT_DOUBLE_EQ(-b.w_dot[0], diff.w_dot[0]); 139 | 140 | // Rotations of 180 deg around single axis will have vanishing 141 | // difference (singularity). 142 | a.q.x() = 1; 143 | EXPECT_DOUBLE_EQ(0.0, diff.rot().norm()); 144 | b.q.x() = 1; 145 | EXPECT_DOUBLE_EQ(0.0, diff.rot().norm()); 146 | } 147 | 148 | int main(int argc, char** argv) 149 | { 150 | testing::InitGoogleTest(&argc, argv); 151 | return RUN_ALL_TESTS(); 152 | } 153 | -------------------------------------------------------------------------------- /cartesian_trajectory_interpolation/test/cartesian_trajectory_segment_test.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2021 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | 31 | //----------------------------------------------------------------------------- 32 | /*!\file cartesian_trajectory_segment_test.cpp 33 | * 34 | * \author Stefan Scherzinger 35 | * \date 2021/05/25 36 | * 37 | */ 38 | //----------------------------------------------------------------------------- 39 | 40 | 41 | #include 42 | 43 | #include 44 | 45 | using namespace cartesian_ros_control; 46 | 47 | TEST(TestCartesianTrajectorySegment, SamplingBeyondBoundariesIsSafe) 48 | { 49 | CartesianState start; 50 | start.p.x() = 1.0; 51 | start.v.x() = 1.0; 52 | start.w.x() = 1.0; 53 | start.v_dot.x() = 1.0; 54 | start.w_dot.x() = 1.0; 55 | double start_time = 0; 56 | 57 | CartesianState end; 58 | end.p.x() = 2.0; 59 | end.v.x() = 2.0; 60 | end.w.x() = 2.0; 61 | end.v_dot.x() = 2.0; 62 | end.w_dot.x() = 2.0; 63 | double end_time = 5.0; 64 | 65 | CartesianTrajectorySegment seg{start_time, start, end_time, end}; 66 | 67 | // Sampling beyond the time boundaries should yield the boundary states with 68 | // zero velocities and zero accelerations. 69 | 70 | CartesianState sample_lower; 71 | seg.sample(-1.0, sample_lower); 72 | 73 | EXPECT_DOUBLE_EQ(start.p.x(), sample_lower.p.x()); 74 | EXPECT_DOUBLE_EQ(0, sample_lower.v.x()); 75 | EXPECT_DOUBLE_EQ(0, sample_lower.w.x()); 76 | EXPECT_DOUBLE_EQ(0, sample_lower.v_dot.x()); 77 | EXPECT_DOUBLE_EQ(0, sample_lower.w_dot.x()); 78 | 79 | CartesianState sample_upper; 80 | seg.sample(6.0, sample_lower); 81 | 82 | EXPECT_DOUBLE_EQ(end.p.x(), sample_lower.p.x()); 83 | EXPECT_DOUBLE_EQ(0, sample_lower.v.x()); 84 | EXPECT_DOUBLE_EQ(0, sample_lower.w.x()); 85 | EXPECT_DOUBLE_EQ(0, sample_lower.v_dot.x()); 86 | EXPECT_DOUBLE_EQ(0, sample_lower.w_dot.x()); 87 | 88 | } 89 | 90 | TEST(TestCartesianTrajectorySegment, DoubleConversionIsIdentity) 91 | { 92 | //-------------------------------------------------------------------------------- 93 | // Cartesian -> Spline -> Cartesian 94 | //-------------------------------------------------------------------------------- 95 | CartesianState c; 96 | 97 | c.v.x() = 1.1; 98 | c.v.y() = 1.2; 99 | c.v.z() = 1.3; 100 | 101 | c.w.x() = 2.1; 102 | c.w.y() = 2.2; 103 | c.w.z() = 2.3; 104 | 105 | c.v_dot.x() = 3.1; 106 | c.v_dot.y() = 3.2; 107 | c.v_dot.z() = 3.3; 108 | 109 | c.w_dot.x() = 4.1; 110 | c.w_dot.y() = 4.2; 111 | c.w_dot.z() = 4.3; 112 | 113 | std::stringstream before; 114 | std::stringstream after; 115 | before << c; 116 | after << convert(convert(c)); 117 | 118 | // Compact check if both `read` the same. 119 | EXPECT_EQ(before.str(), after.str()); 120 | 121 | // The non-initialized case 122 | CartesianState d; 123 | before.clear(); 124 | after.clear(); 125 | before << d; 126 | after << convert(convert(d)); 127 | EXPECT_EQ(before.str(), after.str()); 128 | 129 | } 130 | 131 | TEST(TestCartesianTrajectorySegment, NansYieldEmptySplineVelocities) 132 | { 133 | CartesianState c; 134 | c.p.x() = 1.0; 135 | c.p.y() = 2.0; 136 | c.p.z() = 3.0; 137 | c.q.w() = 4.0; 138 | c.q.x() = 5.0; 139 | c.q.y() = 6.0; 140 | c.q.z() = 7.0; 141 | 142 | c.v.x() = std::nan("0"); 143 | 144 | CartesianTrajectorySegment::SplineState s; 145 | s.position.push_back(1.0); 146 | s.position.push_back(2.0); 147 | s.position.push_back(3.0); 148 | s.position.push_back(4.0); 149 | s.position.push_back(5.0); 150 | s.position.push_back(6.0); 151 | s.position.push_back(7.0); 152 | std::stringstream expected; 153 | expected << s; 154 | 155 | s = convert(c); 156 | std::stringstream actual; 157 | actual << s; 158 | EXPECT_EQ(expected.str(), actual.str()); 159 | } 160 | 161 | 162 | int main(int argc, char** argv) 163 | { 164 | testing::InitGoogleTest(&argc, argv); 165 | return RUN_ALL_TESTS(); 166 | } 167 | -------------------------------------------------------------------------------- /cartesian_interface/include/cartesian_interface/cartesian_state_handle.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2020 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | 31 | //---------------------------------------------------------------------- 32 | /*!\file 33 | * 34 | * \author mauch@fzi.de 35 | * \date 2020-07-01 36 | * 37 | */ 38 | //---------------------------------------------------------------------- 39 | 40 | #pragma once 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | namespace cartesian_ros_control 49 | { 50 | 51 | /** 52 | * @brief A state handle for Cartesian hardware interfaces 53 | * 54 | * Cartesian ROS-controllers can use this handle to read the current Cartesian 55 | * state from the Cartesian HW-interface and use that in their control loops. 56 | * The functionality is analog to how the joint-based handles work: 57 | * Implementers of the hardware_interface::RobotHW class provide a set of 58 | * buffers to this handle upon instantiation and register this handle with an 59 | * instance of the according CartesianStateInterface. 60 | * 61 | */ 62 | class CartesianStateHandle 63 | { 64 | public: 65 | CartesianStateHandle() = default; 66 | CartesianStateHandle(const std::string& ref_frame_id, const std::string& frame_id, const geometry_msgs::Pose* pose, 67 | const geometry_msgs::Twist* twist, const geometry_msgs::Accel* accel, 68 | const geometry_msgs::Accel* jerk) 69 | : frame_id_(frame_id), ref_frame_id_(ref_frame_id), pose_(pose), twist_(twist), accel_(accel), jerk_(jerk) 70 | { 71 | if (!pose) 72 | { 73 | throw hardware_interface::HardwareInterfaceException("Cannot create Cartesian handle for frame '" + frame_id_ + 74 | "'. Pose data pointer is null."); 75 | } 76 | if (!twist) 77 | { 78 | throw hardware_interface::HardwareInterfaceException("Cannot create Cartesian handle for frame '" + frame_id_ + 79 | "'. Twist data pointer is null."); 80 | } 81 | if (!accel) 82 | { 83 | throw hardware_interface::HardwareInterfaceException("Cannot create Cartesian handle for frame '" + frame_id_ + 84 | "'. Accel data pointer is null."); 85 | } 86 | if (!jerk) 87 | { 88 | throw hardware_interface::HardwareInterfaceException("Cannot create Cartesian handle for frame '" + frame_id_ + 89 | "'. Jerk data pointer is null."); 90 | } 91 | } 92 | virtual ~CartesianStateHandle() = default; 93 | 94 | std::string getName() const 95 | { 96 | return frame_id_; 97 | } 98 | geometry_msgs::Pose getPose() const 99 | { 100 | assert(pose_); 101 | return *pose_; 102 | } 103 | geometry_msgs::Twist getTwist() const 104 | { 105 | assert(twist_); 106 | return *twist_; 107 | } 108 | geometry_msgs::Accel getAccel() const 109 | { 110 | assert(accel_); 111 | return *accel_; 112 | } 113 | geometry_msgs::Accel getJerk() const 114 | { 115 | assert(jerk_); 116 | return *jerk_; 117 | } 118 | 119 | private: 120 | std::string frame_id_; 121 | std::string ref_frame_id_; 122 | const geometry_msgs::Pose* pose_; 123 | const geometry_msgs::Twist* twist_; 124 | const geometry_msgs::Accel* accel_; 125 | const geometry_msgs::Accel* jerk_; 126 | }; 127 | 128 | /** 129 | * @brief A Cartesian state interface for hardware_interface::RobotHW abstractions 130 | * 131 | * This interface can be passed to Cartesian ROS-controllers as hardware type during initialization. 132 | * The controllers then obtain read access to the underlying buffers via the \a CartesianStateHandle. 133 | */ 134 | class CartesianStateInterface : public hardware_interface::HardwareResourceManager 135 | { 136 | }; 137 | } // namespace cartesian_ros_control 138 | -------------------------------------------------------------------------------- /cartesian_trajectory_interpolation/test/cartesian_trajectory_test.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2021 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | 31 | //----------------------------------------------------------------------------- 32 | /*!\file cartesian_trajectory_test.cpp 33 | * 34 | * \author Stefan Scherzinger 35 | * \date 2021/05/25 36 | * 37 | */ 38 | //----------------------------------------------------------------------------- 39 | 40 | 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include "Eigen/src/Core/Matrix.h" 47 | #include "cartesian_control_msgs/CartesianTrajectoryPoint.h" 48 | #include "ros/duration.h" 49 | #include 50 | #include 51 | 52 | using namespace cartesian_ros_control; 53 | 54 | TEST(TestCartesianTrajectory, SamplingFromEmptyTrajectoryGivesEmptyState) 55 | { 56 | CartesianState a; 57 | CartesianState b; 58 | 59 | CartesianTrajectory empty_traj; 60 | empty_traj.sample(-0.5, a); 61 | 62 | std::stringstream a_str; 63 | std::stringstream b_str; 64 | a_str << a; 65 | b_str << b; 66 | 67 | EXPECT_EQ(b_str.str(), a_str.str()); 68 | } 69 | 70 | TEST(TestCartesianTrajectory, DefaultQuaternionsGiveValidInterpolation) 71 | { 72 | auto p1 = cartesian_control_msgs::CartesianTrajectoryPoint(); 73 | p1.pose.position.x = 0.0; 74 | p1.pose.position.y = 0.0; 75 | p1.pose.position.z = 0.0; 76 | p1.time_from_start = ros::Duration(0.0); 77 | 78 | auto p2 = cartesian_control_msgs::CartesianTrajectoryPoint(); 79 | p2.pose.position.x = 1.0; 80 | p2.pose.position.y = 1.0; 81 | p2.pose.position.z = 1.0; 82 | p2.time_from_start = ros::Duration(5.0); 83 | 84 | cartesian_control_msgs::CartesianTrajectory traj; 85 | traj.points.push_back(p1); 86 | traj.points.push_back(p2); 87 | 88 | auto c_traj = CartesianTrajectory(traj); 89 | CartesianState c; 90 | c_traj.sample(2.5, c); 91 | 92 | std::stringstream result; 93 | result << c; 94 | 95 | // Check for nan values as a frequent result for invalid, all-zero quaternion operations. 96 | EXPECT_EQ(std::string::npos, result.str().find("nan")); 97 | } 98 | 99 | TEST(TestCartesianTrajectory, NonIncreasingWaypointsInTimeFail) 100 | { 101 | auto p1 = cartesian_control_msgs::CartesianTrajectoryPoint(); 102 | auto p2 = cartesian_control_msgs::CartesianTrajectoryPoint(); 103 | p1.time_from_start = ros::Duration(1.0); 104 | p2.time_from_start = ros::Duration(0.9); 105 | 106 | cartesian_control_msgs::CartesianTrajectory init; 107 | init.points.push_back(p1); 108 | init.points.push_back(p2); 109 | 110 | CartesianTrajectory c_traj; 111 | EXPECT_FALSE(c_traj.init(init)); 112 | } 113 | 114 | TEST(TestCartesianTrajectory, InterpolationGivesPlausibleResults) 115 | { 116 | auto p1 = cartesian_control_msgs::CartesianTrajectoryPoint(); 117 | p1.pose.position.x = 0.0; 118 | p1.pose.position.y = 0.0; 119 | p1.pose.position.z = 0.0; 120 | p1.pose.orientation.x = 0; 121 | p1.pose.orientation.y = 0; 122 | p1.pose.orientation.z = 0; 123 | p1.pose.orientation.w = 1; 124 | p1.time_from_start = ros::Duration(0.0); 125 | 126 | auto p2 = cartesian_control_msgs::CartesianTrajectoryPoint(); 127 | p2.pose.position.x = 1.1; 128 | p2.pose.position.y = 2.2; 129 | p2.pose.position.z = 3.3; 130 | p2.pose.orientation.x = 1; // 180 degrees around x 131 | p2.pose.orientation.y = 0; 132 | p2.pose.orientation.z = 0; 133 | p2.pose.orientation.w = 0; 134 | p2.time_from_start = ros::Duration(10.0); 135 | 136 | cartesian_control_msgs::CartesianTrajectory traj; 137 | traj.points.push_back(p1); 138 | traj.points.push_back(p2); 139 | 140 | auto c_traj = CartesianTrajectory(traj); 141 | CartesianState c; 142 | c_traj.sample(5, c); 143 | 144 | constexpr double pi = 3.14159265358979323846; 145 | 146 | // For the linear case, half the time should give half the values. 147 | EXPECT_DOUBLE_EQ(p2.pose.position.x / 2.0, c.p.x()); 148 | EXPECT_DOUBLE_EQ(p2.pose.position.y / 2.0, c.p.y()); 149 | EXPECT_DOUBLE_EQ(p2.pose.position.z / 2.0, c.p.z()); 150 | EXPECT_DOUBLE_EQ(pi / 2.0, c.rot().norm()); 151 | } 152 | 153 | int main(int argc, char** argv) 154 | { 155 | testing::InitGoogleTest(&argc, argv); 156 | return RUN_ALL_TESTS(); 157 | } 158 | -------------------------------------------------------------------------------- /cartesian_interface/test/cartesian_command_interface_test.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright 2020 FZI Research Center for Information Technology 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // 1. Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | //////////////////////////////////////////////////////////////////////////////// 30 | 31 | //---------------------------------------------------------------------- 32 | /*!\file 33 | * 34 | * \author Felix Exner mauch@fzi.de 35 | * \date 2020-07-02 36 | * 37 | */ 38 | //---------------------------------------------------------------------- 39 | 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | using namespace cartesian_ros_control; 46 | 47 | class CartesianCommandInterfaceTest : public ::testing::Test 48 | { 49 | protected: 50 | std::string reference_frame = "base"; 51 | std::string controlled_frame = "tool0"; 52 | geometry_msgs::Pose pose_buffer; 53 | geometry_msgs::Twist twist_buffer; 54 | geometry_msgs::Accel accel_buffer; 55 | geometry_msgs::Accel jerk_buffer; 56 | CartesianStateHandle state_handle{ reference_frame, controlled_frame, &pose_buffer, 57 | &twist_buffer, &accel_buffer, &jerk_buffer }; 58 | geometry_msgs::Pose pose_cmd_buffer; 59 | geometry_msgs::Twist twist_cmd_buffer; 60 | geometry_msgs::Accel accel_cmd_buffer; 61 | geometry_msgs::Accel jerk_cmd_buffer; 62 | }; 63 | 64 | TEST_F(CartesianCommandInterfaceTest, TestPoseHandleConstructor) 65 | { 66 | EXPECT_NO_THROW(PoseCommandHandle obj(state_handle, &pose_cmd_buffer)); 67 | EXPECT_THROW(PoseCommandHandle obj(state_handle, nullptr), hardware_interface::HardwareInterfaceException); 68 | } 69 | 70 | TEST_F(CartesianCommandInterfaceTest, TestPoseHandleDataHandling) 71 | { 72 | PoseCommandHandle cmd_handle(state_handle, &pose_cmd_buffer); 73 | 74 | PoseCommandInterface iface; 75 | iface.registerHandle(cmd_handle); 76 | 77 | EXPECT_NO_THROW(iface.getHandle(controlled_frame)); 78 | 79 | cmd_handle = iface.getHandle(controlled_frame); 80 | 81 | EXPECT_EQ(controlled_frame, cmd_handle.getName()); 82 | geometry_msgs::Pose new_cmd; 83 | new_cmd.position.x = 1.0; 84 | new_cmd.position.y = 2.0; 85 | new_cmd.position.z = 3.0; 86 | new_cmd.orientation.x = 0.5; 87 | new_cmd.orientation.y = 0.5; 88 | new_cmd.orientation.z = 0.0; 89 | new_cmd.orientation.w = 0.0; 90 | cmd_handle.setCommand(new_cmd); 91 | EXPECT_DOUBLE_EQ(new_cmd.position.x, cmd_handle.getCommand().position.x); 92 | EXPECT_DOUBLE_EQ(new_cmd.position.y, cmd_handle.getCommand().position.y); 93 | EXPECT_DOUBLE_EQ(new_cmd.position.z, cmd_handle.getCommand().position.z); 94 | EXPECT_DOUBLE_EQ(new_cmd.orientation.x, cmd_handle.getCommand().orientation.x); 95 | EXPECT_DOUBLE_EQ(new_cmd.orientation.y, cmd_handle.getCommand().orientation.y); 96 | EXPECT_DOUBLE_EQ(new_cmd.orientation.z, cmd_handle.getCommand().orientation.z); 97 | EXPECT_DOUBLE_EQ(new_cmd.orientation.w, cmd_handle.getCommand().orientation.w); 98 | } 99 | 100 | TEST_F(CartesianCommandInterfaceTest, TestTwistHandleConstructor) 101 | { 102 | EXPECT_NO_THROW(TwistCommandHandle obj(state_handle, &twist_cmd_buffer)); 103 | EXPECT_THROW(TwistCommandHandle obj(state_handle, nullptr), hardware_interface::HardwareInterfaceException); 104 | } 105 | 106 | TEST_F(CartesianCommandInterfaceTest, TestTwistHandleDataHandling) 107 | { 108 | TwistCommandHandle cmd_handle(state_handle, &twist_cmd_buffer); 109 | 110 | TwistCommandInterface iface; 111 | iface.registerHandle(cmd_handle); 112 | 113 | EXPECT_NO_THROW(iface.getHandle(controlled_frame)); 114 | 115 | cmd_handle = iface.getHandle(controlled_frame); 116 | 117 | EXPECT_EQ(controlled_frame, cmd_handle.getName()); 118 | geometry_msgs::Twist new_cmd; 119 | new_cmd.linear.x = 1.0; 120 | new_cmd.linear.y = 2.0; 121 | new_cmd.linear.z = 3.0; 122 | new_cmd.angular.x = 0.5; 123 | new_cmd.angular.y = 0.5; 124 | new_cmd.angular.z = 0.0; 125 | cmd_handle.setCommand(new_cmd); 126 | EXPECT_DOUBLE_EQ(new_cmd.linear.x, cmd_handle.getCommand().linear.x); 127 | EXPECT_DOUBLE_EQ(new_cmd.linear.y, cmd_handle.getCommand().linear.y); 128 | EXPECT_DOUBLE_EQ(new_cmd.linear.z, cmd_handle.getCommand().linear.z); 129 | EXPECT_DOUBLE_EQ(new_cmd.angular.x, cmd_handle.getCommand().angular.x); 130 | EXPECT_DOUBLE_EQ(new_cmd.angular.y, cmd_handle.getCommand().angular.y); 131 | EXPECT_DOUBLE_EQ(new_cmd.angular.z, cmd_handle.getCommand().angular.z); 132 | } 133 | 134 | int main(int argc, char** argv) 135 | { 136 | testing::InitGoogleTest(&argc, argv); 137 | return RUN_ALL_TESTS(); 138 | } 139 | -------------------------------------------------------------------------------- /pass_through_controllers/test/hw_interface/hw_interface.h: -------------------------------------------------------------------------------- 1 | // -- BEGIN LICENSE BLOCK ----------------------------------------------------- 2 | // -- END LICENSE BLOCK ------------------------------------------------------- 3 | 4 | //----------------------------------------------------------------------------- 5 | /*!\file hw_interface.h 6 | * 7 | * \author Stefan Scherzinger 8 | * \date 2020/09/09 9 | * 10 | */ 11 | //----------------------------------------------------------------------------- 12 | 13 | 14 | #pragma once 15 | 16 | // ROS 17 | #include "cartesian_control_msgs/FollowCartesianTrajectoryResult.h" 18 | #include "control_msgs/FollowJointTrajectoryResult.h" 19 | #include "ros/publisher.h" 20 | #include "ros/subscriber.h" 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | // Joint-based control 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | // Cartesian-based control 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | // Dynamic reconfigure 43 | #include 44 | #include 45 | 46 | // Speed scaling 47 | #include 48 | 49 | // Other 50 | #include 51 | #include 52 | #include 53 | #include 54 | 55 | namespace examples { 56 | 57 | class HWInterface : public hardware_interface::RobotHW 58 | { 59 | public: 60 | HWInterface(); 61 | ~HWInterface(); 62 | 63 | void read(const ros::Time& time, const ros::Duration& period) override; 64 | void write(const ros::Time& time, const ros::Duration& period) override; 65 | 66 | private: 67 | /** 68 | * @brief Dummy implementation for joint interpolation on the robot 69 | * 70 | * Passes this trajectory straight to a 71 | * JointTrajectoryController to mimic external interpolation. 72 | * 73 | * @param trajectory The trajectory blob to forward to the vendor driver 74 | */ 75 | void startJointInterpolation(const hardware_interface::JointTrajectory& trajectory); 76 | 77 | /** 78 | * @brief Dummy implementation for Cartesian interpolation on the robot 79 | * 80 | * Passes this trajectory straight to a 81 | * CartesianTrajectoryController to mimic external interpolation. 82 | * 83 | * @param trajectory The trajectory blob to forward to the vendor driver 84 | */ 85 | void startCartesianInterpolation(const hardware_interface::CartesianTrajectory& trajectory); 86 | 87 | /** 88 | * @brief Dummy implementation for canceling a running joint interpolation on the robot 89 | * 90 | * Cancels the active goal of the JointTrajectoryController via preempt request. 91 | */ 92 | void cancelJointInterpolation(); 93 | 94 | /** 95 | * @brief Dummy implementation for canceling a running Cartesian interpolation on the robot 96 | * 97 | * Cancels the active goal of the CartesianTrajectoryController via preempt request. 98 | */ 99 | void cancelCartesianInterpolation(); 100 | 101 | //! Actuated joints in order from base to tip 102 | std::vector joint_names_; 103 | 104 | // Interfaces 105 | cartesian_ros_control::CartesianStateInterface cart_state_interface_; 106 | cartesian_ros_control::PoseCommandInterface pose_cmd_interface_; 107 | hardware_interface::JointStateInterface jnt_state_interface_; 108 | hardware_interface::PositionJointInterface jnt_pos_interface_; 109 | hardware_interface::JointTrajectoryInterface jnt_traj_interface_; 110 | hardware_interface::CartesianTrajectoryInterface cart_traj_interface_; 111 | hardware_interface::SpeedScalingInterface speedsc_interface_; 112 | 113 | // Buffers 114 | std::vector cmd_; 115 | std::vector pos_; 116 | std::vector vel_; 117 | std::vector eff_; 118 | double speed_scaling_; 119 | geometry_msgs::Pose pose_cmd_; 120 | 121 | // Configuration 122 | std::string ref_frame_id_; 123 | std::string frame_id_; 124 | 125 | // Dynamic reconfigure 126 | using SpeedScalingConfig = pass_through_controllers::SpeedScalingConfig; 127 | 128 | /** 129 | * @brief Use dynamic reconfigure to mimic the driver's speed scaling 130 | * 131 | * Note: The speed scaling interface used is not made for thread safety. 132 | * In real driver code, that's not a problem, because robot drivers will 133 | * operate in synchronous read-update-write cycles. Here, we use this 134 | * interface under somewhat unrealistic "dynamic reconfigure" conditions for 135 | * testing purposes. 136 | * 137 | * @param config The speed scaling from 0 to 1.0 138 | * @param level Not used 139 | */ 140 | void dynamicReconfigureCallback(SpeedScalingConfig& config, uint32_t level); 141 | 142 | std::shared_ptr> reconfig_server_; 143 | dynamic_reconfigure::Server::CallbackType callback_type_; 144 | 145 | // States 146 | geometry_msgs::Pose cartesian_pose_; 147 | geometry_msgs::Twist cartesian_twist_; 148 | geometry_msgs::Accel cartesian_accel_; 149 | geometry_msgs::Accel cartesian_jerk_; 150 | 151 | 152 | // Handles 153 | std::vector joint_handles_; 154 | std::vector joint_state_handles_; 155 | 156 | // Robot connection and communication 157 | std::unique_ptr > 158 | joint_based_communication_; 159 | void handleJointFeedback(const control_msgs::FollowJointTrajectoryFeedbackConstPtr& feedback); 160 | 161 | void handleJointDone(const actionlib::SimpleClientGoalState& state, 162 | const control_msgs::FollowJointTrajectoryResultConstPtr& result); 163 | 164 | std::unique_ptr< 165 | actionlib::SimpleActionClient > 166 | cartesian_based_communication_; 167 | void handleCartesianFeedback( 168 | const cartesian_control_msgs::FollowCartesianTrajectoryFeedbackConstPtr& feedback); 169 | 170 | void handleCartesianDone(const actionlib::SimpleClientGoalState& state, 171 | const cartesian_control_msgs::FollowCartesianTrajectoryResultConstPtr& result); 172 | }; 173 | 174 | } // namespace examples 175 | -------------------------------------------------------------------------------- /pass_through_controllers/test/test_forward_joint_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import unittest 3 | import rospy 4 | import actionlib 5 | import sys 6 | from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal, FollowJointTrajectoryResult 7 | from trajectory_msgs.msg import JointTrajectoryPoint 8 | from controller_manager_msgs.srv import SwitchControllerRequest, SwitchController 9 | import tf 10 | import time 11 | import copy 12 | from collections import OrderedDict 13 | 14 | PKG = 'pass_through_controllers' 15 | NAME = 'test_joint_trajectory_forwarding' 16 | 17 | 18 | class IntegrationTest(unittest.TestCase): 19 | 20 | def __init__(self, *args): 21 | super(IntegrationTest, self).__init__(*args) 22 | 23 | rospy.init_node(NAME) 24 | 25 | timeout = rospy.Duration(3) 26 | 27 | self.client = actionlib.SimpleActionClient( 28 | "/hw_interface/forward_joint_trajectories/follow_joint_trajectory", FollowJointTrajectoryAction) 29 | try: 30 | self.client.wait_for_server(timeout) 31 | except rospy.exceptions.ROSException as err: 32 | self.fail("Could not reach controller action. Msg: {}".format(err)) 33 | 34 | self.switch_robot_ctrl = rospy.ServiceProxy('/robot/controller_manager/switch_controller', SwitchController) 35 | try: 36 | self.switch_robot_ctrl.wait_for_service(timeout.to_sec()) 37 | except rospy.exceptions.ROSException as err: 38 | self.fail( 39 | "Could not reach robot controller switch service. Msg: {}".format(err)) 40 | 41 | self.switch_forward_ctrl = rospy.ServiceProxy('/hw_interface/controller_manager/switch_controller', SwitchController) 42 | try: 43 | self.switch_forward_ctrl.wait_for_service(timeout.to_sec()) 44 | except rospy.exceptions.ROSException as err: 45 | self.fail( 46 | "Could not reach hw_interface controller switch service. Msg: {}".format(err)) 47 | 48 | self.listener = tf.TransformListener() 49 | 50 | # Correctly ordered joint names with reference joint state 51 | self.joint_map = OrderedDict() 52 | self.joint_map['joint1'] = 0 53 | self.joint_map['joint2'] = -2.0 54 | self.joint_map['joint3'] = 2.26 55 | self.joint_map['joint4'] = -0.2513274122872 56 | self.joint_map['joint5'] = 1.57 57 | self.joint_map['joint6'] = 0.0 58 | 59 | # Cartesian reference pose for this joint state (x,y,z, qx, qy, qz, qw) 60 | self.p_ref = [0.354, 0.180, 0.390, 0.502, 0.502, 0.498, 0.498] 61 | 62 | def test_normal_execution(self): 63 | """ Test the basic functionality by moving to a defined joint state """ 64 | self.switch_to_joint_control() 65 | 66 | # Move to reference joint state 67 | start_joint_state = FollowJointTrajectoryGoal() 68 | start_joint_state.trajectory.joint_names = self.joint_map.keys() 69 | 70 | start_joint_state.trajectory.points = [ 71 | JointTrajectoryPoint(positions=self.joint_map.values(), time_from_start=rospy.Duration(3.0))] 72 | start_joint_state.goal_time_tolerance = rospy.Duration(1) 73 | self.client.send_goal(start_joint_state) 74 | self.client.wait_for_result() 75 | 76 | time.sleep(1) 77 | self.check_reached() 78 | self.assertEqual(self.client.get_result().error_code, 79 | FollowJointTrajectoryResult.SUCCESSFUL) 80 | 81 | def test_mixed_joint_order(self): 82 | """ Test whether a mixed-up joint order gets executed correctly """ 83 | self.switch_to_joint_control() 84 | 85 | # Move to reference joint state with different joint order 86 | joint_names = ['joint6', 'joint4', 'joint3', 'joint1', 'joint5', 'joint2'] 87 | q_start = [self.joint_map[i] for i in joint_names] 88 | 89 | start_joint_state = FollowJointTrajectoryGoal() 90 | start_joint_state.trajectory.joint_names = joint_names 91 | 92 | start_joint_state.trajectory.points = [ 93 | JointTrajectoryPoint(positions=q_start, time_from_start=rospy.Duration(3.0))] 94 | start_joint_state.goal_time_tolerance = rospy.Duration(1) 95 | self.client.send_goal(start_joint_state) 96 | self.client.wait_for_result() 97 | 98 | time.sleep(1) 99 | self.check_reached() 100 | self.assertEqual(self.client.get_result().error_code, 101 | FollowJointTrajectoryResult.SUCCESSFUL) 102 | 103 | def test_wrong_joint_names(self): 104 | """ Test whether a trajectory with wrong joint names fails """ 105 | self.switch_to_joint_control() 106 | 107 | # Unknown joint names 108 | joint_names = ['a', 'b', 'c', 'd', 'e', 'f'] 109 | 110 | start_joint_state = FollowJointTrajectoryGoal() 111 | start_joint_state.trajectory.joint_names = joint_names 112 | 113 | start_joint_state.trajectory.points = [ 114 | JointTrajectoryPoint(positions=self.joint_map.values(), time_from_start=rospy.Duration(3.0))] 115 | start_joint_state.goal_time_tolerance = rospy.Duration(1) 116 | self.client.send_goal(start_joint_state) 117 | self.client.wait_for_result() 118 | 119 | time.sleep(1) 120 | self.assertEqual(self.client.get_result().error_code, 121 | FollowJointTrajectoryResult.INVALID_GOAL) 122 | 123 | def switch_to_joint_control(self): 124 | """ Assume possibly running Cartesian controllers""" 125 | 126 | # Prepare robot dummy 127 | srv = SwitchControllerRequest() 128 | srv.stop_controllers = ['cartesian_trajectory_controller'] 129 | srv.start_controllers = ['joint_trajectory_controller'] 130 | srv.strictness = SwitchControllerRequest.BEST_EFFORT 131 | self.switch_robot_ctrl(srv) 132 | 133 | # Prepare passthrough controller 134 | srv = SwitchControllerRequest() 135 | srv.stop_controllers = ['forward_cartesian_trajectories'] 136 | srv.start_controllers = ['forward_joint_trajectories'] 137 | srv.strictness = SwitchControllerRequest.BEST_EFFORT 138 | self.switch_forward_ctrl(srv) 139 | 140 | def check_reached(self): 141 | """ Check whether we reached our target within TF lookup accuracy """ 142 | try: 143 | (trans, rot) = self.listener.lookupTransform('base_link', 'tool0', rospy.Time(0)) 144 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 145 | pass 146 | n = 3 # places accuracy 147 | self.assertAlmostEqual(self.p_ref[0], trans[0], n) 148 | self.assertAlmostEqual(self.p_ref[1], trans[1], n) 149 | self.assertAlmostEqual(self.p_ref[2], trans[2], n) 150 | self.assertAlmostEqual(self.p_ref[3], rot[0], n) 151 | self.assertAlmostEqual(self.p_ref[4], rot[1], n) 152 | self.assertAlmostEqual(self.p_ref[5], rot[2], n) 153 | self.assertAlmostEqual(self.p_ref[6], rot[3], n) 154 | 155 | 156 | if __name__ == '__main__': 157 | import rostest 158 | rostest.run(PKG, NAME, IntegrationTest, sys.argv) 159 | -------------------------------------------------------------------------------- /twist_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(twist_controller) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | cartesian_interface 12 | controller_interface 13 | dynamic_reconfigure 14 | geometry_msgs 15 | hardware_interface 16 | realtime_tools 17 | roscpp 18 | ) 19 | 20 | ## System dependencies are found with CMake's conventions 21 | # find_package(Boost REQUIRED COMPONENTS system) 22 | 23 | 24 | ## Uncomment this if the package has a setup.py. This macro ensures 25 | ## modules and global scripts declared therein get installed 26 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 27 | # catkin_python_setup() 28 | 29 | ################################################ 30 | ## Declare ROS messages, services and actions ## 31 | ################################################ 32 | 33 | ## To declare and build messages, services or actions from within this 34 | ## package, follow these steps: 35 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 36 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 37 | ## * In the file package.xml: 38 | ## * add a build_depend tag for "message_generation" 39 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 40 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 41 | ## but can be declared for certainty nonetheless: 42 | ## * add a exec_depend tag for "message_runtime" 43 | ## * In this file (CMakeLists.txt): 44 | ## * add "message_generation" and every package in MSG_DEP_SET to 45 | ## find_package(catkin REQUIRED COMPONENTS ...) 46 | ## * add "message_runtime" and every package in MSG_DEP_SET to 47 | ## catkin_package(CATKIN_DEPENDS ...) 48 | ## * uncomment the add_*_files sections below as needed 49 | ## and list every .msg/.srv/.action file to be processed 50 | ## * uncomment the generate_messages entry below 51 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 52 | 53 | ## Generate messages in the 'msg' folder 54 | # add_message_files( 55 | # FILES 56 | # Message1.msg 57 | # Message2.msg 58 | # ) 59 | 60 | ## Generate services in the 'srv' folder 61 | # add_service_files( 62 | # FILES 63 | # Service1.srv 64 | # Service2.srv 65 | # ) 66 | 67 | ## Generate actions in the 'action' folder 68 | # add_action_files( 69 | # FILES 70 | # Action1.action 71 | # Action2.action 72 | # ) 73 | 74 | ## Generate added messages and services with any dependencies listed here 75 | # generate_messages( 76 | # DEPENDENCIES 77 | # std_msgs # Or other packages containing msgs 78 | # ) 79 | 80 | ################################################ 81 | ## Declare ROS dynamic reconfigure parameters ## 82 | ################################################ 83 | 84 | ## To declare and build dynamic reconfigure parameters within this 85 | ## package, follow these steps: 86 | ## * In the file package.xml: 87 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 88 | ## * In this file (CMakeLists.txt): 89 | ## * add "dynamic_reconfigure" to 90 | ## find_package(catkin REQUIRED COMPONENTS ...) 91 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 92 | ## and list every .cfg file to be processed 93 | 94 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 95 | generate_dynamic_reconfigure_options( 96 | cfg/TwistController.cfg 97 | ) 98 | 99 | ################################### 100 | ## catkin specific configuration ## 101 | ################################### 102 | ## The catkin_package macro generates cmake config files for your package 103 | ## Declare things to be passed to dependent projects 104 | ## INCLUDE_DIRS: uncomment this if your package contains header files 105 | ## LIBRARIES: libraries you create in this project that dependent projects also need 106 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 107 | ## DEPENDS: system dependencies of this project that dependent projects also need 108 | catkin_package( 109 | INCLUDE_DIRS include 110 | LIBRARIES ${PROJECT_NAME} 111 | CATKIN_DEPENDS 112 | controller_interface 113 | dynamic_reconfigure 114 | geometry_msgs 115 | hardware_interface 116 | realtime_tools 117 | roscpp 118 | #DEPENDS system_lib 119 | ) 120 | 121 | ########### 122 | ## Build ## 123 | ########### 124 | 125 | ## Specify additional locations of header files 126 | ## Your package locations should be listed before other locations 127 | include_directories( 128 | include 129 | ${catkin_INCLUDE_DIRS} 130 | ) 131 | 132 | ## Declare a C++ library 133 | add_library(${PROJECT_NAME} 134 | src/twist_controller.cpp 135 | ) 136 | 137 | 138 | 139 | ## Add cmake target dependencies of the library 140 | ## as an example, code may need to be generated before libraries 141 | ## either from message generation or dynamic reconfigure 142 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Declare a C++ executable 145 | ## With catkin_make all packages are built within a single CMake context 146 | ## The recommended prefix ensures that target names across packages don't collide 147 | # add_executable(${PROJECT_NAME}_node src/twist_controller_node.cpp) 148 | 149 | ## Rename C++ executable without prefix 150 | ## The above recommended prefix causes long target names, the following renames the 151 | ## target back to the shorter version for ease of user use 152 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 153 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 154 | 155 | ## Add cmake target dependencies of the executable 156 | ## same as for the library above 157 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 158 | 159 | ## Specify libraries to link a library or executable target against 160 | target_link_libraries(${PROJECT_NAME} 161 | ${catkin_LIBRARIES} 162 | ) 163 | 164 | ############# 165 | ## Install ## 166 | ############# 167 | 168 | # all install targets should use catkin DESTINATION variables 169 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 170 | 171 | ## Mark executable scripts (Python etc.) for installation 172 | ## in contrast to setup.py, you can choose the destination 173 | # catkin_install_python(PROGRAMS 174 | # scripts/my_python_script 175 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 176 | # ) 177 | 178 | ## Mark executables for installation 179 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 180 | # install(TARGETS ${PROJECT_NAME}_node 181 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 182 | # ) 183 | 184 | ## Mark libraries for installation 185 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 186 | install(TARGETS ${PROJECT_NAME} 187 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 188 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 189 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 190 | ) 191 | 192 | ## Mark cpp header files for installation 193 | install(DIRECTORY include/${PROJECT_NAME}/ 194 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 195 | FILES_MATCHING PATTERN "*.h" 196 | PATTERN ".svn" EXCLUDE 197 | ) 198 | 199 | ## Mark other files for installation (e.g. launch and bag files, etc.) 200 | install(FILES 201 | ${PROJECT_NAME}_plugin.xml 202 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 203 | ) 204 | 205 | ############# 206 | ## Testing ## 207 | ############# 208 | 209 | ## Add gtest based cpp test target and link libraries 210 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_twist_controller.cpp) 211 | # if(TARGET ${PROJECT_NAME}-test) 212 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 213 | # endif() 214 | 215 | ## Add folders to be run by python nosetests 216 | # catkin_add_nosetests(test) 217 | -------------------------------------------------------------------------------- /cartesian_trajectory_controller/test/test_controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import unittest 4 | import copy 5 | import rospy 6 | import actionlib 7 | from actionlib_msgs.msg import GoalStatus 8 | import time 9 | from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal 10 | from cartesian_control_msgs.msg import ( 11 | FollowCartesianTrajectoryAction, 12 | FollowCartesianTrajectoryGoal, 13 | FollowCartesianTrajectoryResult, 14 | CartesianTrajectoryPoint) 15 | from trajectory_msgs.msg import JointTrajectoryPoint 16 | from controller_manager_msgs.srv import SwitchControllerRequest, SwitchController 17 | 18 | PKG = 'cartesian_trajectory_controller' 19 | NAME = 'test_controller' 20 | 21 | 22 | class IntegrationTest(unittest.TestCase): 23 | def __init__(self, *args): 24 | super(IntegrationTest, self).__init__(*args) 25 | 26 | rospy.init_node('test_controller') 27 | 28 | timeout = rospy.Duration(3) 29 | 30 | self.set_joints = actionlib.SimpleActionClient( 31 | "/joint_trajectory_controller/follow_joint_trajectory", FollowJointTrajectoryAction) 32 | try: 33 | self.set_joints.wait_for_server(timeout) 34 | except rospy.exceptions.ROSException as err: 35 | self.fail("Could not reach controller action. Msg: {}".format(err)) 36 | 37 | self.cart_client = actionlib.SimpleActionClient( 38 | 'cartesian_trajectory_controller/follow_cartesian_trajectory', FollowCartesianTrajectoryAction) 39 | try: 40 | self.cart_client.wait_for_server(timeout) 41 | except rospy.exceptions.ROSException as err: 42 | self.fail("Could not reach controller action. Msg: {}".format(err)) 43 | 44 | self.pub_trajectory = actionlib.SimpleActionClient( 45 | 'cartesian_trajectory_publisher/follow_cartesian_trajectory', FollowCartesianTrajectoryAction) 46 | if not self.cart_client.wait_for_server(timeout): 47 | self.fail("Could not reach trajectory publisher action.") 48 | 49 | self.switch = rospy.ServiceProxy('/controller_manager/switch_controller', SwitchController) 50 | try: 51 | self.switch.wait_for_service(timeout.to_sec()) 52 | except rospy.exceptions.ROSException as err: 53 | self.fail( 54 | "Could not reach controller switch service. Msg: {}".format(err)) 55 | 56 | def test_normal_execution(self): 57 | """ Test the basic functionality on a straight line """ 58 | self.move_to_start() 59 | self.switch_to_cartesian_control() 60 | self.move_square(self.cart_client) 61 | self.assertEqual(self.cart_client.get_result().error_code, 62 | FollowCartesianTrajectoryResult.SUCCESSFUL) 63 | 64 | def test_preemption(self): 65 | """ Test whether the preemption mechanism works correctly """ 66 | self.move_to_start() 67 | self.switch_to_cartesian_control() 68 | self.move_square(self.cart_client, wait_for_result=False) 69 | time.sleep(3) # stop somewhere during execution 70 | self.cart_client.cancel_goal() 71 | time.sleep(1) 72 | self.assertEqual(self.cart_client.get_state(), 73 | GoalStatus.PREEMPTED) 74 | 75 | def test_invalid_goals(self): 76 | """ Test whether invalid goals are rejected correctly """ 77 | 78 | self.move_to_start() 79 | self.switch_to_cartesian_control() 80 | 81 | # Time from start is not strictly increasing. 82 | # This also covers points without time_from_start (they are all implicitly 0). 83 | p1 = CartesianTrajectoryPoint() 84 | p1.time_from_start = rospy.Duration(1.0) 85 | p2 = CartesianTrajectoryPoint() 86 | p2.time_from_start = rospy.Duration(0.9999) 87 | goal = FollowCartesianTrajectoryGoal() 88 | goal.trajectory.points.append(p1) 89 | goal.trajectory.points.append(p2) 90 | self.cart_client.send_goal(goal) 91 | self.cart_client.wait_for_result() 92 | self.assertEqual(self.cart_client.get_result().error_code, 93 | FollowCartesianTrajectoryResult.INVALID_GOAL) 94 | 95 | def test_trajectory_publishing(self): 96 | """ Test whether trajectory publishing works """ 97 | 98 | self.move_to_start() 99 | 100 | # Activate trajectory publishing 101 | srv = SwitchControllerRequest() 102 | srv.stop_controllers = [] 103 | srv.start_controllers = ['cartesian_trajectory_publisher'] 104 | srv.strictness = SwitchControllerRequest.BEST_EFFORT 105 | self.switch(srv) 106 | 107 | self.move_square(self.pub_trajectory) 108 | 109 | # Deactivate trajectory publishing to not spam the other tests. 110 | srv = SwitchControllerRequest() 111 | srv.stop_controllers = ['cartesian_trajectory_publisher'] 112 | srv.start_controllers = [] 113 | srv.strictness = SwitchControllerRequest.BEST_EFFORT 114 | self.switch(srv) 115 | 116 | self.assertEqual(self.pub_trajectory.get_result().error_code, 117 | FollowCartesianTrajectoryResult.SUCCESSFUL) 118 | 119 | def move_to_start(self): 120 | srv = SwitchControllerRequest() 121 | srv.stop_controllers = ['cartesian_trajectory_controller'] 122 | srv.start_controllers = ['joint_trajectory_controller'] 123 | srv.strictness = SwitchControllerRequest.BEST_EFFORT 124 | self.switch(srv) 125 | 126 | q_start = [0, -2.0, 2.26, -0.2513274122872, 1.57, 0.0] # From base to tip 127 | start_joint_state = FollowJointTrajectoryGoal() 128 | start_joint_state.trajectory.joint_names = [ 129 | 'joint1', 130 | 'joint2', 131 | 'joint3', 132 | 'joint4', 133 | 'joint5', 134 | 'joint6'] 135 | 136 | start_joint_state.trajectory.points = [ 137 | JointTrajectoryPoint(positions=q_start, time_from_start=rospy.Duration(3.0))] 138 | self.set_joints.send_goal( 139 | start_joint_state) 140 | self.set_joints.wait_for_result() 141 | 142 | def switch_to_cartesian_control(self): 143 | srv = SwitchControllerRequest() 144 | srv.stop_controllers = ['joint_trajectory_controller'] 145 | srv.start_controllers = ['cartesian_trajectory_controller'] 146 | srv.strictness = SwitchControllerRequest.BEST_EFFORT 147 | self.switch(srv) 148 | 149 | def move_square(self, action_client, wait_for_result=True): 150 | """ Follow a 20 cm square in the y-z plane within 10 sec """ 151 | 152 | # Cartesian end-effector pose that corresponds to the start joint 153 | # configuration 154 | start = CartesianTrajectoryPoint() 155 | start.pose.position.x = 0.354 156 | start.pose.position.y = 0.180 157 | start.pose.position.z = 0.390 158 | start.pose.orientation.x = 0.502 159 | start.pose.orientation.y = 0.502 160 | start.pose.orientation.z = 0.498 161 | start.pose.orientation.w = 0.498 162 | 163 | duration = 10 164 | 165 | p1 = copy.deepcopy(start) 166 | p1.pose.position.y += 0.2 167 | p1.time_from_start = rospy.Duration(1.0 / 4 * duration) 168 | 169 | p2 = copy.deepcopy(p1) 170 | p2.pose.position.z += 0.2 171 | p2.time_from_start = rospy.Duration(2.0 / 4 * duration) 172 | 173 | p3 = copy.deepcopy(p2) 174 | p3.pose.position.y -= 0.2 175 | p3.time_from_start = rospy.Duration(3.0 / 4 * duration) 176 | 177 | p4 = copy.deepcopy(p3) 178 | p4.pose.position.z -= 0.2 179 | p4.time_from_start = rospy.Duration(4.0 / 4 * duration) 180 | 181 | goal = FollowCartesianTrajectoryGoal() 182 | goal.trajectory.points.append(p1) 183 | goal.trajectory.points.append(p2) 184 | goal.trajectory.points.append(p3) 185 | goal.trajectory.points.append(p4) 186 | 187 | action_client.send_goal(goal) 188 | if wait_for_result: 189 | action_client.wait_for_result() 190 | 191 | 192 | if __name__ == '__main__': 193 | import rostest 194 | rostest.run(PKG, NAME, IntegrationTest, sys.argv) 195 | -------------------------------------------------------------------------------- /cartesian_trajectory_controller/test/visualization.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /TF1/Frames1 9 | - /RobotModel1/Links1 10 | - /Pose1 11 | Splitter Ratio: 0.511482 12 | Tree Height: 747 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679016 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.0299999993 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Class: rviz/TF 56 | Enabled: true 57 | Frame Timeout: 15 58 | Frames: 59 | All Enabled: false 60 | base_link: 61 | Value: true 62 | link1: 63 | Value: true 64 | link2: 65 | Value: true 66 | link3: 67 | Value: true 68 | link4: 69 | Value: true 70 | link5: 71 | Value: true 72 | link6: 73 | Value: true 74 | sensor_link: 75 | Value: false 76 | space_mouse: 77 | Value: true 78 | tool0: 79 | Value: true 80 | world: 81 | Value: true 82 | Marker Scale: 0.300000012 83 | Name: TF 84 | Show Arrows: false 85 | Show Axes: true 86 | Show Names: false 87 | Tree: 88 | world: 89 | base_link: 90 | link1: 91 | link2: 92 | link3: 93 | link4: 94 | link5: 95 | link6: 96 | sensor_link: 97 | {} 98 | tool0: 99 | space_mouse: 100 | {} 101 | Update Interval: 0 102 | Value: true 103 | - Alpha: 1 104 | Class: rviz/RobotModel 105 | Collision Enabled: false 106 | Enabled: true 107 | Links: 108 | All Links Enabled: true 109 | Expand Joint Details: false 110 | Expand Link Details: false 111 | Expand Tree: false 112 | Link Tree Style: Links in Alphabetic Order 113 | base_link: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | Value: true 118 | link1: 119 | Alpha: 1 120 | Show Axes: false 121 | Show Trail: false 122 | Value: true 123 | link2: 124 | Alpha: 1 125 | Show Axes: false 126 | Show Trail: false 127 | Value: true 128 | link3: 129 | Alpha: 1 130 | Show Axes: false 131 | Show Trail: false 132 | Value: true 133 | link4: 134 | Alpha: 1 135 | Show Axes: false 136 | Show Trail: false 137 | Value: true 138 | link5: 139 | Alpha: 1 140 | Show Axes: false 141 | Show Trail: false 142 | Value: true 143 | link6: 144 | Alpha: 1 145 | Show Axes: false 146 | Show Trail: false 147 | Value: true 148 | sensor_link: 149 | Alpha: 1 150 | Show Axes: false 151 | Show Trail: false 152 | space_mouse: 153 | Alpha: 1 154 | Show Axes: false 155 | Show Trail: false 156 | Value: true 157 | tool0: 158 | Alpha: 1 159 | Show Axes: false 160 | Show Trail: false 161 | Value: true 162 | world: 163 | Alpha: 1 164 | Show Axes: false 165 | Show Trail: false 166 | Name: RobotModel 167 | Robot Description: robot_description 168 | TF Prefix: "" 169 | Update Interval: 0 170 | Value: true 171 | Visual Enabled: true 172 | - Alpha: 1 173 | Axes Length: 0.0500000007 174 | Axes Radius: 0.0199999996 175 | Class: rviz/Pose 176 | Color: 255; 25; 0 177 | Enabled: true 178 | Head Length: 0.300000012 179 | Head Radius: 0.100000001 180 | Name: Pose 181 | Shaft Length: 1 182 | Shaft Radius: 0.0500000007 183 | Shape: Axes 184 | Topic: /cartesian_trajectory_publisher/reference_pose 185 | Unreliable: false 186 | Value: true 187 | Enabled: true 188 | Global Options: 189 | Background Color: 116; 116; 116 190 | Default Light: true 191 | Fixed Frame: world 192 | Frame Rate: 30 193 | Name: root 194 | Tools: 195 | - Class: rviz/Interact 196 | Hide Inactive Objects: true 197 | - Class: rviz/MoveCamera 198 | - Class: rviz/Select 199 | - Class: rviz/FocusCamera 200 | - Class: rviz/Measure 201 | - Class: rviz/SetInitialPose 202 | Topic: /initialpose 203 | - Class: rviz/SetGoal 204 | Topic: /move_base_simple/goal 205 | - Class: rviz/PublishPoint 206 | Single click: true 207 | Topic: /clicked_point 208 | Value: true 209 | Views: 210 | Current: 211 | Class: rviz/Orbit 212 | Distance: 2.64989567 213 | Enable Stereo Rendering: 214 | Stereo Eye Separation: 0.0599999987 215 | Stereo Focal Distance: 1 216 | Swap Stereo Eyes: false 217 | Value: false 218 | Focal Point: 219 | X: -0.0034658527 220 | Y: -0.304883659 221 | Z: 0.146290645 222 | Focal Shape Fixed Size: true 223 | Focal Shape Size: 0.0500000007 224 | Invert Z Axis: false 225 | Name: Current View 226 | Near Clip Distance: 0.00999999978 227 | Pitch: 0.249796987 228 | Target Frame: 229 | Value: Orbit (rviz) 230 | Yaw: 0.795410216 231 | Saved: ~ 232 | Window Geometry: 233 | Displays: 234 | collapsed: false 235 | Height: 1028 236 | Hide Left Dock: false 237 | Hide Right Dock: false 238 | QMainWindow State: 000000ff00000000fd0000000400000000000001e10000037afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000037a000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002dcfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002dc000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007470000003efc0100000002fb0000000800540069006d00650100000000000007470000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005600000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 239 | Selection: 240 | collapsed: false 241 | Time: 242 | collapsed: false 243 | Tool Properties: 244 | collapsed: false 245 | Views: 246 | collapsed: false 247 | Width: 1863 248 | X: 57 249 | Y: 24 250 | -------------------------------------------------------------------------------- /cartesian_trajectory_controller/README.md: -------------------------------------------------------------------------------- 1 | # Cartesian Trajectory Controller 2 | A simple Cartesian trajectory controller that uses the new Cartesian trajectory definition. 3 | 4 | ## Rationale 5 | This controller shall get you started with executing Cartesian trajectories on your robot. 6 | It implements a simple action server for `FollowCartesianTrajectory`. There are currently two ways of execution, depending on the interface provided by the robot. 7 | 8 | The table below highlights possible applications. 9 | 10 | | Hardware interface | Primary application | 11 | | -------- | -------- | 12 | | ``PoseCommandInterface`` | You want spline interpolation in ROS but the OEMs driver to take care of inverse kinematics (IK). This variant requires the new Cartesian interfaces. 13 | | ``PositionJointInterface`` | You want spline interpolation in ROS and implement your own IK solver. The provided example uses the established Weighted Levenberg-Marquardt solver form KDL. This variant is compatible with current ROS-control so that you need not change your RobotHW abstraction. 14 | 15 | 16 | ## Controller .yaml 17 | An example config for the Universal Robots UR10 looks like this: 18 | ```yaml 19 | # This controller uses a Cartesian pose interface for control. 20 | # Inverse Kinematics is handled by the robot itself. 21 | pose_cartesian_traj_controller: 22 | type: "pose_controllers/CartesianTrajectoryController" 23 | 24 | # This type uses the names according to UR driver convention. 25 | # They need not neccessarily exist in the robot_description 26 | # and are only used to get the correct control handle from the 27 | # hardware interface. 28 | base: "base" 29 | tip: "tool0_controller" 30 | 31 | joints: 32 | - shoulder_pan_joint 33 | - shoulder_lift_joint 34 | - elbow_joint 35 | - wrist_1_joint 36 | - wrist_2_joint 37 | - wrist_3_joint 38 | 39 | # This controller uses a joint position interface for control. 40 | # Inverse Kinematics is handled by the controller. 41 | jnt_cartesian_traj_controller: 42 | type: "position_controllers/CartesianTrajectoryController" 43 | 44 | # This type requires valid URDF links and they are used to 45 | # set-up an internal kinematics chain. Make sure that they correspond to the drivers' frames. 46 | # In this case, tool0 and tool0_controller are identical, but the latter 47 | # one does not exist in the URDF description. 48 | base: "base" 49 | tip: "tool0" 50 | 51 | joints: 52 | - shoulder_pan_joint 53 | - shoulder_lift_joint 54 | - elbow_joint 55 | - wrist_1_joint 56 | - wrist_2_joint 57 | - wrist_3_joint 58 | 59 | ``` 60 | ## Interpolation between waypoints 61 | This Cartesian trajectory controller builds on the [joint_trajectory_controller](http://wiki.ros.org/joint_trajectory_controller)'s quintic spline library. 62 | 63 | That library is internally wrapped to work in Cartesian space dimensions and inherits the essential interpolation mechanisms: 64 | 65 | - If only poses are specified, linear interpolation will be used. 66 | - If poses and twists are specified, a cubic spline will be used. 67 | - If poses, twists and accelerations are specified, a quintic spline will be used. 68 | - If two succeeding waypoints have different specifications 69 | (eg. waypoint 1 is pose-only, end waypoint 2 is pose-twist), the lowest common specification will be used 70 | (pose-only in the example). 71 | 72 | --- 73 | **Note**: 74 | By means of the default ROS message initializer, the waypoints' twist and acceleration fields will be zero-initialized. 75 | Those zeros will be interpreted as _intended_ boundary conditions on velocity and accelerations level. 76 | As a consequence, specifying pose-only Cartesian trajectories will lead to smooth stops in each waypoint by default (= quintic interpolation with zero velocity/acceleration boundaries). 77 | 78 | --- 79 | 80 | If you want linear behavior for your pose-only Cartesian trajectory, i.e. you don't want those stops, you should mark at least one of the twist field values as _uninitialized_ by setting a `NaN` equivalent. 81 | Here are two examples how to do that in client code: 82 | 83 | In C++: 84 | ```c++ 85 | #include 86 | 87 | cartesian_control_msgs::CartesianTrajectoryPoint p; 88 | // Fill p.pose here 89 | p.twist.linear.x = std::nan("0"); // this will declare p.twist uninitialized. 90 | ``` 91 | 92 | or in Python: 93 | ```python 94 | from cartesian_control_msgs.msg import CartesianTrajectoryPoint 95 | 96 | p = CartesianTrajectoryPoint() 97 | # Fill p.pose here 98 | 99 | p.twist.linear.x = float('NaN') # this will declare p.twist uninitialized. 100 | ``` 101 | 102 | Here are further details: 103 | - Marking twist as uninitialized automatically marks acceleration uninitialized. 104 | - Uninitializing only individual fields is currently not supported. A single `NaN` in any Cartesian dimension uninitializes the whole twist/acceleration quantity. 105 | - Note that if you intentionally specify non-zero boundaries for some of the twist/acceleration dimensions, the other dimensions are implicitly 0 by their default initializer. 106 | 107 | ## Customizing Inverse Kinematics 108 | 109 | Plugins allow you to implement your own (possibly more advanced) Inverse Kinematics algorithm. For instance, you may wish 110 | to use additional sensors in your environment and react online to objects and collisions. 111 | 112 | Take a look at the provided *example_solver* on how to use the plugin mechanism for your custom implementation. 113 | You can then specify the *ik_solver* at startup in form of a controller-local parameter in the .yaml controller config file: 114 | 115 | ```yaml 116 | jnt_cartesian_traj_controller: 117 | type: "position_controllers/CartesianTrajectoryController" 118 | ik_solver: "example_solver" 119 | ... 120 | 121 | ``` 122 | 123 | ## Visualizing Cartesian trajectories 124 | 125 | For some use cases, it might be advantageous to visualize Cartesian trajectories before commanding an actual robot. 126 | Especially when specifying twist and acceleration boundary conditions for the waypoints, 127 | the resulting motion might not be intuitive and you might want to check _visually_ if that's what you expect. 128 | There's an easy mechanism that allows you to do that. 129 | 130 | Add an additional `CartesianTrajectoryPublisher` as read-only 131 | controller to your set of ROS controllers. It offers the `CartesianTrajectoryController`'s 132 | action interface but does not claim resources nor actuates the robot's 133 | joints. It just publishes the trajectory's interpolated Cartesian state 134 | as reference `PoseStamped` and `TwistStamped` messages. 135 | 136 | The controller specification in .yaml could look like this: 137 | 138 | ```yaml 139 | visualize_cartesian_trajectories: 140 | type: "cartesian_trajectory_publisher/CartesianTrajectoryPublisher" 141 | base: "base_link" 142 | tip: "tool0" 143 | joints: 144 | - shoulder_pan_joint 145 | - shoulder_lift_joint 146 | - elbow_joint 147 | - wrist_1_joint 148 | - wrist_2_joint 149 | - wrist_3_joint 150 | ``` 151 | 152 | --- 153 | **Note**: It does _not_ check reachability of the robot end-effector by means of inverse kinematics, nor collisions. 154 | Its purpose is to offer an easy way for users to test whether their Cartesian trajectories 155 | _look_ as expected. 156 | 157 | --- 158 | 159 | You can then use the controller's `visualize_cartesian_trajectories/follow_cartesian_trajectory` action interface and inspect 160 | the trajectory by adding a _Pose_ visualization (`geometry_msgs::PoseStamped`) in RViz for the *visualize_cartesian_trajectories/reference_pose* topic: 161 | 162 | ![](./doc/resources/rviz_visualize_cartesian_trajectories.png) 163 | 164 | 165 | 166 | *** 167 | 171 | 172 | 173 | rosin_logo 175 | 176 | 177 | Supported by ROSIN - ROS-Industrial Quality-Assured Robot Software Components. 178 | More information: rosin-project.eu 179 | 180 | eu_flag 182 | 183 | This project has received funding from the European Union’s Horizon 2020 184 | research and innovation programme under grant agreement no. 732287. 185 | -------------------------------------------------------------------------------- /pass_through_controllers/README.md: -------------------------------------------------------------------------------- 1 | # Pass-Through Controllers 2 | A package for ROS-controllers and HW Interface mechanisms to enable forwarding of trajectories (both 3 | joint-based and Cartesian) to the robot for interpolation. 4 | 5 | ## Rationale 6 | The idea behind these controllers is to enable forwarding (pass through) incoming trajectories directly 7 | to the robot and let the OEM driver-side take care of interpolating between the waypoints. 8 | This is useful if your application demands industrial scale trajectory execution, and you prefer not to interpolate on the ROS side. 9 | Having the complete trajectory available on the driver side can be beneficial regarding smoothness of execution and avoid 10 | issues related to sending (streaming) ad hoc commands, as is classically done with current ROS-control approaches. 11 | The controllers implement simple action servers for trajectory execution (both Cartesian and joint-based). 12 | They are meant to be light-weight and their functionality is deliberately 13 | limited to starting trajectories, canceling them, and providing real-time 14 | feedback on progress. 15 | 16 | Note that most of the work has to be done in the hardware abstraction of the robot. 17 | And, unfortunately, this is very robot specific and hard to generalize. It's 18 | somewhat similar to the _read()_ and _write()_ functions of ROS-control, which 19 | need to implement robot-specific protocols. 20 | This package provides the necessary HW interfaces to implement this feature. 21 | 22 | ## Controller .yaml 23 | An example config could look like this: 24 | ```yaml 25 | # A controller to forward joint trajectories to the robot 26 | forward_joint_trajectories: 27 | type: "joint_trajectory_controllers/PassThroughController" 28 | joints: 29 | - joint1 30 | - joint2 31 | - joint3 32 | - joint4 33 | - joint5 34 | - joint6 35 | 36 | # A controller to forward joint trajectories to the robot 37 | forward_cartesian_trajectories: 38 | type: "cartesian_trajectory_controllers/PassThroughController" 39 | joints: 40 | - joint1 41 | - joint2 42 | - joint3 43 | - joint4 44 | - joint5 45 | - joint6 46 | ``` 47 | 48 | ## Examples 49 | Call `` roslaunch pass_through_controllers demo.launch `` 50 | to get a first impression and inspect how things work. 51 | The ``cartesian_trajectory_action_client.py`` and the 52 | ``joint_trajectory_action_client.py`` from the _script_ folder trigger random 53 | motion. 54 | 55 | 56 | ## Adapting your RobotHW 57 | The pass_through_controllers expect either a ``JointTrajectoryInterface`` or a ``CartesianTrajectoryInterface``, depending on the trajectory to forward. 58 | Registering them in your RobotHW could look like this: 59 | ```c++ 60 | 61 | #include 62 | 63 | class YourRobot : public hardware_interface::RobotHW 64 | { 65 | ... 66 | 67 | // Callbacks for the pass_through_controllers' events 68 | void startJointInterpolation(const hardware_interface::JointTrajectory& trajectory); 69 | void startCartesianInterpolation(const hardware_interface::CartesianTrajectory& trajectory); 70 | void cancelJointInterpolation(); 71 | void cancelCartesianInterpolation(); 72 | 73 | // New HW interfaces for trajectory forwarding 74 | hardware_interface::JointTrajectoryInterface jnt_traj_interface_; 75 | hardware_interface::CartesianTrajectoryInterface cart_traj_interface_; 76 | 77 | // Command and feedback buffers 78 | hardware_interface::JointTrajectory jnt_traj_cmd_; 79 | hardware_interface::JointTrajectoryFeedback jnt_traj_feedback_; 80 | hardware_interface::CartesianTrajectory cart_traj_cmd_; 81 | hardware_interface::CartesianTrajectoryFeedback cart_traj_feedback_; 82 | } 83 | ``` 84 | And in the implementation: 85 | ```c++ 86 | 87 | YourRobot::YourRobot() 88 | { 89 | ... 90 | // Initialize and register joint trajectory command handles for PassThroughControllers 91 | hardware_interface::JointTrajectoryHandle joint_trajectory_handle = 92 | hardware_interface::JointTrajectoryHandle( 93 | &jnt_traj_cmd_, 94 | &jnt_traj_feedback_, 95 | std::bind(&HWInterface::startJointInterpolation, this, std::placeholders::_1), 96 | std::bind(&HWInterface::cancelJointInterpolation, this)); 97 | 98 | jnt_traj_interface_.registerHandle(joint_trajectory_handle); 99 | registerInterface(&jnt_traj_interface_); 100 | 101 | // Initialize and register Cartesian trajectory command handles for PassThroughControllers 102 | hardware_interface::CartesianTrajectoryHandle cartesian_trajectory_handle = 103 | hardware_interface::CartesianTrajectoryHandle( 104 | &cart_traj_cmd_, 105 | &cart_traj_feedback_, 106 | std::bind(&HWInterface::startCartesianInterpolation, this, std::placeholders::_1), 107 | std::bind(&HWInterface::cancelCartesianInterpolation, this)); 108 | 109 | cart_traj_interface_.registerHandle(cartesian_trajectory_handle); 110 | registerInterface(&cart_traj_interface_); 111 | ... 112 | } 113 | 114 | void YourRobot::startJointInterpolation(const hardware_interface::JointTrajectory& trajectory) 115 | { 116 | // Robot-specific implementation here 117 | } 118 | 119 | void YourRobot::startCartesianInterpolation(const hardware_interface::CartesianTrajectory& trajectory) 120 | { 121 | // Robot-specific implementation here 122 | } 123 | 124 | void YourRobot::cancelJointInterpolation() 125 | { 126 | // Robot-specific implementation here 127 | } 128 | 129 | void YourRobot::cancelCartesianInterpolation() 130 | { 131 | // Robot-specific implementation here 132 | } 133 | 134 | 135 | ``` 136 | 137 | The pass_through_controllers check for these interfaces and call the registered 138 | callbacks upon receiving new action goals or preemption requests. The implementation is robot-specific. 139 | 140 | In order to provide feedback for the pass_through_controllers' action clients, 141 | you should periodically fill the feedback buffers: 142 | ```c++ 143 | // Handle name is a convention 144 | jnt_traj_interface_.getHandle("joint_trajectory_handle").setFeedback(feedback); 145 | 146 | // Handle name is a convention 147 | cart_traj_interface_.getHandle("cartesian_trajectory_handle").setFeedback(feedback); 148 | 149 | ``` 150 | 151 | ## FAQ 152 | The passthrough controllers forward the trajectories to the robot for interpolation. 153 | On the ROS side, the possibilities to interfere with a running action are fairly limited and you can only start or preempt the trajectory execution. 154 | On the robot side, however, there will be additional ways to interfere with the execution that mostly depend on the OEM robot program and the implementation of the respective driver. 155 | The passthrough controllers support the additional concept of speed scaling, which can be used to pause trajectories. 156 | 157 | The following examples explain the passthrough controllers' behavior with the Universal Robots [`ur_robot_driver`](http://wiki.ros.org/ur_robot_driver). 158 | 159 | ### What happens when you hit the emergency stop? 160 | The trajectory execution is paused in this case by internally setting the speed scaling to zero. Once the program resumes, the trajectory execution continues. 161 | The passthrough controllers will wait until the execution finishes but will report an execution failure due to missing the waypoints' time requirements if a goal tolerance is specified. 162 | 163 | ### What happens when you power-off the system? 164 | This aborts the current trajectory execution. 165 | 166 | ### What happens when the robot goes into protective stop? 167 | This is handled similar to hitting the emergency stop. The speed scaling is set 168 | to zero and the trajectory execution continues once the protective 169 | stop is resolved. 170 | 171 | ### What happens when you stop the program? 172 | This stops all ROS-controllers and aborts the current trajectory execution. 173 | 174 | *** 175 | 179 | 180 | 181 | rosin_logo 183 | 184 | 185 | Supported by ROSIN - ROS-Industrial Quality-Assured Robot Software Components. 186 | More information: rosin-project.eu 187 | 188 | eu_flag 190 | 191 | This project has received funding from the European Union’s Horizon 2020 192 | research and innovation programme under grant agreement no. 732287. 193 | --------------------------------------------------------------------------------