Skip to content

Commit 24fd577

Browse files
author
Felix Exner
committed
Add scaled_jtc from ur_controllers
This is a filtered version from https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver which in turn came from https://github.com/UniversalRobots/Universal_Robots_ROS_Driver originally.
2 parents 8dd2b5f + e1b9337 commit 24fd577

10 files changed

+741
-0
lines changed

‎ur_controllers/CMakeLists.txt‎

Lines changed: 94 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,94 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
project(ur_controllers)
3+
4+
# Default to C++14
5+
if(NOT CMAKE_CXX_STANDARD)
6+
set(CMAKE_CXX_STANDARD 14)
7+
endif()
8+
9+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
10+
add_compile_options(-Wall -Wextra)
11+
endif()
12+
13+
find_package(ament_cmake REQUIRED)
14+
find_package(angles REQUIRED)
15+
find_package(control_msgs REQUIRED)
16+
find_package(controller_interface REQUIRED)
17+
find_package(geometry_msgs REQUIRED)
18+
find_package(joint_trajectory_controller REQUIRED)
19+
find_package(pluginlib REQUIRED)
20+
find_package(rclcpp_lifecycle REQUIRED)
21+
find_package(rcutils REQUIRED)
22+
find_package(realtime_tools REQUIRED)
23+
find_package(sensor_msgs REQUIRED)
24+
find_package(std_msgs REQUIRED)
25+
find_package(std_srvs REQUIRED)
26+
find_package(ur_client_library REQUIRED)
27+
find_package(ur_dashboard_msgs REQUIRED)
28+
find_package(ur_msgs REQUIRED)
29+
30+
set(THIS_PACKAGE_INCLUDE_DEPENDS
31+
angles
32+
control_msgs
33+
controller_interface
34+
geometry_msgs
35+
joint_trajectory_controller
36+
pluginlib
37+
rclcpp_lifecycle
38+
rcutils
39+
realtime_tools
40+
sensor_msgs
41+
std_msgs
42+
std_srvs
43+
ur_client_library
44+
ur_dashboard_msgs
45+
ur_msgs
46+
)
47+
48+
include_directories(include)
49+
50+
add_library(${PROJECT_NAME} SHARED
51+
src/scaled_joint_trajectory_controller.cpp
52+
src/speed_scaling_state_broadcaster.cpp
53+
src/force_torque_sensor_broadcaster.cpp
54+
src/gpio_controller.cpp)
55+
56+
target_include_directories(${PROJECT_NAME} PRIVATE
57+
include
58+
)
59+
60+
ament_target_dependencies(${PROJECT_NAME}
61+
${THIS_PACKAGE_INCLUDE_DEPENDS}
62+
)
63+
64+
# prevent pluginlib from using boost
65+
target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
66+
pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml)
67+
68+
install(TARGETS ${PROJECT_NAME}
69+
ARCHIVE DESTINATION lib
70+
LIBRARY DESTINATION lib
71+
RUNTIME DESTINATION bin
72+
)
73+
74+
install(DIRECTORY include/
75+
DESTINATION include
76+
)
77+
78+
install(FILES controller_plugins.xml
79+
DESTINATION share/${PROJECT_NAME}
80+
)
81+
82+
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
83+
ament_export_include_directories(include)
84+
ament_export_libraries(${PROJECT_NAME})
85+
86+
ament_export_include_directories(
87+
include
88+
)
89+
90+
ament_export_libraries(
91+
${PROJECT_NAME}
92+
)
93+
94+
ament_package()

‎ur_controllers/README.md‎

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
# ur_controllers
2+
3+
This package contains controllers and hardware interface for `ros_control` that are special to the UR
4+
robot family. Currently this contains
5+
6+
* A **speed_scaling_interface** to read the value of the current speed scaling into controllers.
7+
* A **scaled_joint_command_interface** that provides access to joint values and commands in
8+
combination with the speed scaling value.
9+
* A **speed_scaling_state_controller** that publishes the current execution speed as reported by
10+
the robot to a topic interface. Values are floating points between 0 and 1.
11+
* A **scaled_joint_trajectory_controller** that is similar to the *joint_trajectory_controller*,
12+
but it uses the speed scaling reported by the robot to reduce progress in the trajectory.
13+
14+
## About this package
15+
This package contains controllers not being available in the default `ros_control` set. They are
16+
created to support more features offered by the UR robot family. Any of these controllers are
17+
example implementations for certain features and are intended to be generalized and merged
18+
into the default `ros_control` controller set at some future point.
19+
20+
## Controller description
21+
This packages offers a couple of specific controllers that will be explained in the following
22+
sections.
23+
### ur_controllers/SpeedScalingStateBroadcaster
24+
This controller publishes the current actual execution speed as reported by the robot. Values are
25+
floating points between 0 and 1.
26+
27+
In the [`ur_robot_driver`](../ur_robot_driver) this is calculated by multiplying the two [RTDE](https://www.universal-robots.com/articles/ur/real-time-data-exchange-rtde-guide/) data
28+
fields `speed_scaling` (which should be equal to the value shown by the speed slider position on the
29+
teach pendant) and `target_speed_fraction` (Which is the fraction to which execution gets slowed
30+
down by the controller).
31+
### position_controllers/ScaledJointTrajectoryController and velocity_controllers/ScaledJointTrajectoryController
32+
These controllers work similar to the well-known
33+
[`joint_trajectory_controller`](http://wiki.ros.org/joint_trajectory_controller).
34+
35+
However, they are extended to handle the robot's execution speed specifically. Because the default
36+
`joint_trajectory_controller` would interpolate the trajectory with the configured time constraints (ie: always assume maximum velocity and acceleration supported by the robot),
37+
this could lead to significant path deviation due to multiple reasons:
38+
- The speed slider on the robot might not be at 100%, so motion commands sent from ROS would
39+
effectively get scaled down resulting in a slower execution.
40+
- The robot could scale down motions based on configured safety limits resulting in a slower motion
41+
than expected and therefore not reaching the desired target in a control cycle.
42+
- Motions might not be executed at all, e.g. because the robot is E-stopped or in a protective stop
43+
- Motion commands sent to the robot might not be interpreted, e.g. because there is no
44+
[`external_control`](https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#prepare-the-robot)
45+
program node running on the robot controller.
46+
- The program interpreting motion commands could be paused.
47+
48+
The following plot illustrates the problem:
49+
![Trajectory execution with default trajectory controller](doc/traj_without_speed_scaling.png
50+
"Trajectory execution with default trajectory controller")
51+
52+
The graph shows a trajectory with one joint being moved to a target point and back to its starting
53+
point. As the joint's speed is limited to a very low setting on the teach pendant, speed scaling
54+
(black line) activates and limits the joint speed (green line). As a result, the target
55+
trajectory (light blue) doesn't get executed by the robot, but instead the pink trajectory is executed.
56+
The vertical distance between the light blue line and the pink line is the path error in each
57+
control cycle. We can see that the path deviation gets above 300 degrees at some point and the
58+
target point at -6 radians never gets reached.
59+
60+
All of the cases mentioned above are addressed by the scaled trajectory versions. Trajectory execution
61+
can be transparently scaled down using the speed slider on the teach pendant without leading to
62+
additional path deviations. Pausing the program or hitting the E-stop effectively leads to
63+
`speed_scaling` being 0 meaning the trajectory will not be continued until the program is continued.
64+
This way, trajectory executions can be explicitly paused and continued.
65+
66+
With the scaled version of the trajectory controller the example motion shown in the previous diagram becomes:
67+
![Trajectory execution with scaled_joint_trajectory_controller](doc/traj_with_speed_scaling.png
68+
"Trajectory execution with scaled_joint_trajectory_controller")
69+
70+
The deviation between trajectory interpolation on the ROS side and actual robot execution stays minimal and the
71+
robot reaches the intermediate setpoint instead of returning "too early" as in the example above.
72+
73+
Under the hood this is implemented by proceeding the trajectory not by a full time step but only by
74+
the fraction determined by the current speed scaling. If speed scaling is currently at 50% then
75+
interpolation of the current control cycle will start half a time step after the beginning of the
76+
previous control cycle.

‎ur_controllers/controller_plugins.xml‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
<library path="ur_controllers">
2+
<class name="ur_controllers/ForceTorqueStateBroadcaster" type="ur_controllers::ForceTorqueStateBroadcaster" base_class_type="controller_interface::ControllerInterface">
3+
<description>
4+
The force torque state controller publishes the current force and torques
5+
</description>
6+
</class>
7+
<class name="ur_controllers/SpeedScalingStateBroadcaster" type="ur_controllers::SpeedScalingStateBroadcaster" base_class_type="controller_interface::ControllerInterface">
8+
<description>
9+
This controller publishes the readings of all available speed scaling factors.
10+
</description>
11+
</class>
12+
<class name="ur_controllers/ScaledJointTrajectoryController" type="ur_controllers::ScaledJointTrajectoryController" base_class_type="controller_interface::ControllerInterface">
13+
<description>
14+
Scaled position-based joint trajectory controller.
15+
</description>
16+
</class>
17+
<class name="ur_controllers/GPIOController" type="ur_controllers::GPIOController" base_class_type="controller_interface::ControllerInterface">
18+
<description>
19+
This controller publishes the Tool IO.
20+
</description>
21+
</class>
22+
</library>

‎ur_controllers/doc/traj_with_speed_scaling.png‎

102 KB
Loading

‎ur_controllers/doc/traj_without_speed_scaling.png‎

105 KB
Loading

‎ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
// Copyright 2019, FZI Forschungszentrum Informatik
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
//----------------------------------------------------------------------
16+
/*!\file
17+
*
18+
* \author Marvin Große Besselmann [email protected]
19+
* \date 2021-02-18
20+
*
21+
*/
22+
//----------------------------------------------------------------------
23+
#ifndef UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_
24+
#define UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_
25+
26+
#include "angles/angles.h"
27+
#include "joint_trajectory_controller/joint_trajectory_controller.hpp"
28+
#include "joint_trajectory_controller/trajectory.hpp"
29+
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
30+
#include "rclcpp/time.hpp"
31+
#include "rclcpp/duration.hpp"
32+
33+
namespace ur_controllers
34+
{
35+
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
36+
37+
class ScaledJointTrajectoryController : public joint_trajectory_controller::JointTrajectoryController
38+
{
39+
public:
40+
ScaledJointTrajectoryController() = default;
41+
~ScaledJointTrajectoryController() override = default;
42+
43+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
44+
45+
CallbackReturn on_activate(const rclcpp_lifecycle::State& state) override;
46+
47+
controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
48+
49+
protected:
50+
struct TimeData
51+
{
52+
TimeData() : time(0.0), period(rclcpp::Duration::from_nanoseconds(0.0)), uptime(0.0)
53+
{
54+
}
55+
rclcpp::Time time;
56+
rclcpp::Duration period;
57+
rclcpp::Time uptime;
58+
};
59+
60+
private:
61+
double scaling_factor_;
62+
realtime_tools::RealtimeBuffer<TimeData> time_data_;
63+
};
64+
} // namespace ur_controllers
65+
66+
#endif // UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_

‎ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp‎

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
// Copyright 2019, FZI Forschungszentrum Informatik
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
//----------------------------------------------------------------------
16+
/*!\file
17+
*
18+
* \author Marvin Große Besselmann [email protected]
19+
* \date 2021-02-10
20+
*
21+
*/
22+
//----------------------------------------------------------------------
23+
24+
#ifndef UR_CONTROLLERS__SPEED_SCALING_STATE_BROADCASTER_HPP_
25+
#define UR_CONTROLLERS__SPEED_SCALING_STATE_BROADCASTER_HPP_
26+
27+
#include <memory>
28+
#include <string>
29+
#include <vector>
30+
31+
#include "controller_interface/controller_interface.hpp"
32+
#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
33+
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
34+
#include "rclcpp/time.hpp"
35+
#include "rclcpp/duration.hpp"
36+
#include "std_msgs/msg/float64.hpp"
37+
38+
namespace ur_controllers
39+
{
40+
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
41+
42+
class SpeedScalingStateBroadcaster : public controller_interface::ControllerInterface
43+
{
44+
public:
45+
SpeedScalingStateBroadcaster();
46+
47+
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
48+
49+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
50+
51+
controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
52+
53+
CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
54+
55+
CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
56+
57+
CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
58+
59+
CallbackReturn on_init() override;
60+
61+
protected:
62+
std::vector<std::string> sensor_names_;
63+
double publish_rate_;
64+
65+
std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Float64>> speed_scaling_state_publisher_;
66+
std_msgs::msg::Float64 speed_scaling_state_msg_;
67+
};
68+
} // namespace ur_controllers
69+
#endif // UR_CONTROLLERS__SPEED_SCALING_STATE_BROADCASTER_HPP_

‎ur_controllers/package.xml‎

Lines changed: 43 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,43 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>ur_controllers</name>
5+
<version>0.0.0</version>
6+
<description>Provides controllers that use the speed scaling interface of Universal Robots.</description>
7+
8+
<author>Marvin Große Besselmann</author>
9+
<author>Lovro Ivanov</author>
10+
11+
<maintainer email="[email protected]">Marvin Große Besselmann</maintainer>
12+
<maintainer email="[email protected]">Lovro Ivanov</maintainer>
13+
<maintainer email="[email protected]">Denis Stogl</maintainer>
14+
<maintainer email="[email protected]">Andy Zelenak</maintainer>
15+
16+
<license>Apache License 2.0</license>
17+
18+
<url type="bugtracker">https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues</url>
19+
<url type="repository">https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver</url>
20+
21+
<buildtool_depend>ament_cmake</buildtool_depend>
22+
23+
<depend>angles</depend>
24+
<depend>control_msgs</depend>
25+
<depend>controller_interface</depend>
26+
<depend>geometry_msgs</depend>
27+
<depend>joint_trajectory_controller</depend>
28+
<depend>pluginlib</depend>
29+
<depend>rclcpp_lifecycle</depend>
30+
<depend>rcutils</depend>
31+
<depend>realtime_tools</depend>
32+
<depend>sensor_msgs</depend>
33+
<depend>std_msgs</depend>
34+
<depend>std_srvs</depend>
35+
<!-- TODO: rosdep cannot find ur_client_library -->
36+
<depend>ur_client_library</depend>
37+
<depend>ur_dashboard_msgs</depend>
38+
<depend>ur_msgs</depend>
39+
40+
<export>
41+
<build_type>ament_cmake</build_type>
42+
</export>
43+
</package>

0 commit comments

Comments
 (0)