Skip to content

Commit 4df673d

Browse files
MGBlaMGBla
authored
Scaled Joint Trajectory Controller (#43)
* Barebone structure inherited from the ros2 controll joint trajectory controller * Collect speed scaling factor from hardware interface * Copied update cycle of JointTrajectoryController * Implemented basic speed scaling functionality Calculate the scaled trajectory time Use adjusted trajectory time for lookup of next trajectory segment instead of node_->now() * Fixed clang tidy issues * Moved code from header into cpp file * Fixed typo and adjusted error msg Co-authored-by: Marvin Große Besselmann <[email protected]>
1 parent 169cd03 commit 4df673d

File tree

4 files changed

+260
-6
lines changed

4 files changed

+260
-6
lines changed

‎ur_controllers/controller_plugins.xml‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,4 +9,9 @@
99
This controller publishes the readings of all available speed scaling factors.
1010
</description>
1111
</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>
1217
</library>

‎ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.h‎

Lines changed: 32 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -19,19 +19,47 @@
1919
//----------------------------------------------------------------------
2020
/*!\file
2121
*
22-
* \author Felix Exner exner@fzi.de
23-
* \date 2019-04-18
22+
* \author Marvin Große Besselmann grosse@fzi.de
23+
* \date 2021-02-18
2424
*
2525
*/
2626
//----------------------------------------------------------------------
2727
#ifndef UR_CONTROLLERS_SCALED_TRAJECTORY_CONTROLLER_H_INCLUDED
2828
#define UR_CONTROLLERS_SCALED_TRAJECTORY_CONTROLLER_H_INCLUDED
2929

30+
#include "joint_trajectory_controller/joint_trajectory_controller.hpp"
31+
#include "joint_trajectory_controller/trajectory.hpp"
32+
#include "angles/angles.h"
33+
3034
namespace ur_controllers
3135
{
32-
template <class SegmentImpl, class HardwareInterface>
33-
class ScaledJointTrajectoryController
36+
class ScaledJointTrajectoryController : public joint_trajectory_controller::JointTrajectoryController
3437
{
38+
public:
39+
ScaledJointTrajectoryController() = default;
40+
~ScaledJointTrajectoryController() override = default;
41+
42+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
43+
44+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
45+
on_activate(const rclcpp_lifecycle::State& state) override;
46+
47+
controller_interface::return_type update() override;
48+
49+
protected:
50+
struct TimeData
51+
{
52+
TimeData() : time(0.0), period(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_;
3563
};
3664
} // namespace ur_controllers
3765

‎ur_controllers/src/scaled_joint_trajectory_controller.cpp‎

Lines changed: 204 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -19,10 +19,212 @@
1919
//----------------------------------------------------------------------
2020
/*!\file
2121
*
22-
* \author Felix Exner exner@fzi.de
23-
* \date 2019-04-18
22+
* \author Marvin Große Besselmann grosse@fzi.de
23+
* \date 2021-02-18
2424
*
2525
*/
2626
//----------------------------------------------------------------------
2727

2828
#include "ur_controllers/scaled_joint_trajectory_controller.h"
29+
30+
namespace ur_controllers
31+
{
32+
controller_interface::InterfaceConfiguration ScaledJointTrajectoryController::state_interface_configuration() const
33+
{
34+
controller_interface::InterfaceConfiguration conf;
35+
conf = JointTrajectoryController::state_interface_configuration();
36+
conf.names.push_back("speed_scaling/speed_scaling_factor");
37+
return conf;
38+
}
39+
40+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
41+
ScaledJointTrajectoryController::on_activate(const rclcpp_lifecycle::State& state)
42+
{
43+
TimeData time_data;
44+
time_data.time = node_->now();
45+
time_data.period = rclcpp::Duration(0, 0);
46+
time_data.uptime = node_->now();
47+
time_data_.initRT(time_data);
48+
return JointTrajectoryController::on_activate(state);
49+
}
50+
51+
controller_interface::return_type ScaledJointTrajectoryController::update()
52+
{
53+
if (state_interfaces_.back().get_name() == "speed_scaling")
54+
{
55+
scaling_factor_ = state_interfaces_.back().get_value();
56+
}
57+
else
58+
{
59+
RCLCPP_ERROR(get_node()->get_logger(), "Speed scaling interface not found in hardware interface.");
60+
}
61+
62+
if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
63+
{
64+
if (!is_halted)
65+
{
66+
halt();
67+
is_halted = true;
68+
}
69+
return controller_interface::return_type::SUCCESS;
70+
}
71+
72+
auto resize_joint_trajectory_point = [](trajectory_msgs::msg::JointTrajectoryPoint& point, size_t size) {
73+
point.positions.resize(size);
74+
point.velocities.resize(size);
75+
point.accelerations.resize(size);
76+
};
77+
auto compute_error_for_joint = [](JointTrajectoryPoint& error, int index, const JointTrajectoryPoint& current,
78+
const JointTrajectoryPoint& desired) {
79+
// error defined as the difference between current and desired
80+
error.positions[index] = angles::shortest_angular_distance(current.positions[index], desired.positions[index]);
81+
error.velocities[index] = desired.velocities[index] - current.velocities[index];
82+
error.accelerations[index] = 0.0;
83+
};
84+
85+
// Check if a new external message has been received from nonRT threads
86+
auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg();
87+
auto new_external_msg = traj_msg_external_point_ptr_.readFromRT();
88+
if (current_external_msg != *new_external_msg)
89+
{
90+
fill_partial_goal(*new_external_msg);
91+
sort_to_local_joint_order(*new_external_msg);
92+
traj_external_point_ptr_->update(*new_external_msg);
93+
}
94+
95+
JointTrajectoryPoint state_current, state_desired, state_error;
96+
const auto joint_num = joint_names_.size();
97+
resize_joint_trajectory_point(state_current, joint_num);
98+
99+
// current state update
100+
for (auto index = 0ul; index < joint_num; ++index)
101+
{
102+
state_current.positions[index] = joint_position_state_interface_[index].get().get_value();
103+
state_current.velocities[index] = joint_velocity_state_interface_[index].get().get_value();
104+
state_current.accelerations[index] = 0.0;
105+
}
106+
state_current.time_from_start.set__sec(0);
107+
108+
// currently carrying out a trajectory
109+
if (traj_point_active_ptr_ && !(*traj_point_active_ptr_)->has_trajectory_msg())
110+
{
111+
// if sampling the first time, set the point before you sample
112+
if (!(*traj_point_active_ptr_)->is_sampled_already())
113+
{
114+
(*traj_point_active_ptr_)->set_point_before_trajectory_msg(node_->now(), state_current);
115+
}
116+
resize_joint_trajectory_point(state_error, joint_num);
117+
118+
// Main Speed scaling difference...
119+
// Adjust time with scaling factor
120+
TimeData time_data;
121+
time_data.time = node_->now();
122+
rcl_duration_value_t period = (time_data.time - time_data_.readFromRT()->time).nanoseconds();
123+
time_data.period = rclcpp::Duration(scaling_factor_ * period);
124+
time_data.uptime = time_data_.readFromRT()->uptime + time_data.period;
125+
rclcpp::Time traj_time = time_data_.readFromRT()->uptime + rclcpp::Duration(period);
126+
time_data_.writeFromNonRT(time_data);
127+
128+
// find segment for current timestamp
129+
joint_trajectory_controller::TrajectoryPointConstIter start_segment_itr, end_segment_itr;
130+
const bool valid_point =
131+
(*traj_point_active_ptr_)->sample(traj_time, state_desired, start_segment_itr, end_segment_itr);
132+
133+
if (valid_point)
134+
{
135+
bool abort = false;
136+
bool outside_goal_tolerance = false;
137+
const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end();
138+
for (auto index = 0ul; index < joint_num; ++index)
139+
{
140+
// set values for next hardware write()
141+
joint_position_command_interface_[index].get().set_value(state_desired.positions[index]);
142+
compute_error_for_joint(state_error, index, state_current, state_desired);
143+
144+
if (before_last_point &&
145+
!check_state_tolerance_per_joint(state_error, index, default_tolerances_.state_tolerance[index], true))
146+
{
147+
abort = true;
148+
}
149+
// past the final point, check that we end up inside goal tolerance
150+
if (!before_last_point &&
151+
!check_state_tolerance_per_joint(state_error, index, default_tolerances_.goal_state_tolerance[index], true))
152+
{
153+
outside_goal_tolerance = true;
154+
}
155+
}
156+
157+
if (rt_active_goal_)
158+
{
159+
// send feedback
160+
auto feedback = std::make_shared<FollowJTrajAction::Feedback>();
161+
feedback->header.stamp = node_->now();
162+
feedback->joint_names = joint_names_;
163+
164+
feedback->actual = state_current;
165+
feedback->desired = state_desired;
166+
feedback->error = state_error;
167+
rt_active_goal_->setFeedback(feedback);
168+
169+
// check abort
170+
if (abort || outside_goal_tolerance)
171+
{
172+
auto result = std::make_shared<FollowJTrajAction::Result>();
173+
174+
if (abort)
175+
{
176+
RCLCPP_WARN(node_->get_logger(), "Aborted due to state tolerance violation");
177+
result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED);
178+
}
179+
else if (outside_goal_tolerance)
180+
{
181+
RCLCPP_WARN(node_->get_logger(), "Aborted due to goal tolerance violation");
182+
result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED);
183+
}
184+
rt_active_goal_->setAborted(result);
185+
rt_active_goal_.reset();
186+
}
187+
188+
// check goal tolerance
189+
if (!before_last_point)
190+
{
191+
if (!outside_goal_tolerance)
192+
{
193+
auto res = std::make_shared<FollowJTrajAction::Result>();
194+
res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL);
195+
rt_active_goal_->setSucceeded(res);
196+
rt_active_goal_.reset();
197+
198+
RCLCPP_INFO(node_->get_logger(), "Goal reached, success!");
199+
}
200+
else if (default_tolerances_.goal_time_tolerance != 0.0)
201+
{
202+
// if we exceed goal_time_toleralance set it to aborted
203+
const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time();
204+
const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start;
205+
206+
// TODO This will break in speed scaling we have to discuss how to handle the goal
207+
// time when the robot scales itself down.
208+
const double difference = node_->now().seconds() - traj_end.seconds();
209+
if (difference > default_tolerances_.goal_time_tolerance)
210+
{
211+
auto result = std::make_shared<FollowJTrajAction::Result>();
212+
result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED);
213+
rt_active_goal_->setAborted(result);
214+
rt_active_goal_.reset();
215+
RCLCPP_WARN(node_->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", difference);
216+
}
217+
}
218+
}
219+
}
220+
}
221+
}
222+
223+
publish_state(state_desired, state_current, state_error);
224+
return controller_interface::return_type::SUCCESS;
225+
}
226+
227+
} // namespace ur_controllers
228+
229+
#include "pluginlib/class_list_macros.hpp"
230+
PLUGINLIB_EXPORT_CLASS(ur_controllers::ScaledJointTrajectoryController, controller_interface::ControllerInterface)

‎ur_ros2_control_demos/config/ur_ros2_control.yaml‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,9 @@ controller_manager:
1717
force_torque_state_controller:
1818
type: ur_controllers/ForceTorqueStateController
1919

20+
scaled_joint_trajectory_controller:
21+
type: ur_controllers/ScaledJointTrajectoryController
22+
2023
ur_joint_trajectory_controller:
2124
ros__parameters:
2225
joints:
@@ -33,6 +36,22 @@ ur_joint_trajectory_controller:
3336
stopped_velocity_tolerance: 0.0
3437
goal_time: 0.0
3538

39+
scaled_joint_trajectory_controller:
40+
ros__parameters:
41+
joints:
42+
- shoulder_pan_joint
43+
- shoulder_lift_joint
44+
- elbow_joint
45+
- wrist_1_joint
46+
- wrist_2_joint
47+
- wrist_3_joint
48+
state_publish_rate: 100.0
49+
action_monitor_rate: 20.0
50+
allow_partial_joints_goal: false
51+
constraints:
52+
stopped_velocity_tolerance: 0.2
53+
goal_time: 0.0
54+
3655
speed_scaling_state_controller:
3756
ros__parameters:
3857
state_publish_rate: 100.0

0 commit comments

Comments
 (0)