Skip to content

Commit

Permalink
[JTC] Convert lambda to class functions (backport #945) (#1015)
Browse files Browse the repository at this point in the history
  • Loading branch information
@mergify
mergify[bot] authored Mar 13, 2024
1 parent 880e5b3 commit 1e634f3
Show file tree
Hide file tree
Showing 5 changed files with 261 additions and 44 deletions.
35 changes: 33 additions & 2 deletions ...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -210,6 +210,20 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
void goal_accepted_callback(
std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle);

using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint;

/**
* Computes the error for a specific joint in the trajectory.
*
* @param[out] error The computed error for the joint.
* @param[in] index The index of the joint in the trajectory.
* @param[in] current The current state of the joints.
* @param[in] desired The desired state of the joints.
*/
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
void compute_error_for_joint(
JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current,
const JointTrajectoryPoint & desired) const;
// fill trajectory_msg so it matches joints controlled by this controller
// positions set to current position, velocities, accelerations and efforts to 0.0
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
Expand All @@ -218,7 +232,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
// sorts the joints of the incoming message to our local order
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
void sort_to_local_joint_order(
std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg);
std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg) const;
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const;
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
Expand Down Expand Up @@ -252,7 +266,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
bool has_active_trajectory() const;

using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint;
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
void publish_state(
const rclcpp::Time & time, const JointTrajectoryPoint & desired_state,
Expand Down Expand Up @@ -284,6 +297,24 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
void resize_joint_trajectory_point_command(
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);

/**
* @brief Assigns the values from a trajectory point interface to a joint interface.
*
* @tparam T The type of the joint interface.
* @param[out] joint_interface The reference_wrapper to assign the values to
* @param[in] trajectory_point_interface Containing the values to assign.
* @todo: Use auto in parameter declaration with c++20
*/
template <typename T>
void assign_interface_from_point(
const T & joint_interface, const std::vector<double> & trajectory_point_interface)
{
for (size_t index = 0; index < dof_; ++index)
{
joint_interface[index].get().set_value(trajectory_point_interface[index]);
}
}
};

} // namespace joint_trajectory_controller
Expand Down
2 changes: 1 addition & 1 deletion joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,7 +147,7 @@ inline bool check_state_tolerance_per_joint(
if (show_errors)
{
const auto logger = rclcpp::get_logger("tolerances");
RCLCPP_ERROR(logger, "State tolerances failed for joint %d:", joint_idx);
RCLCPP_ERROR(logger, "State tolerances failed for joint %lu:", joint_idx);

if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position)
{
Expand Down
70 changes: 29 additions & 41 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,35 +131,6 @@ controller_interface::return_type JointTrajectoryController::update(
}
}

auto compute_error_for_joint = [&](
JointTrajectoryPoint & error, size_t index,
const JointTrajectoryPoint & current,
const JointTrajectoryPoint & desired)
{
// error defined as the difference between current and desired
if (joints_angle_wraparound_[index])
{
// if desired, the shortest_angular_distance is calculated, i.e., the error is
// normalized between -pi<error<pi
error.positions[index] =
angles::shortest_angular_distance(current.positions[index], desired.positions[index]);
}
else
{
error.positions[index] = desired.positions[index] - current.positions[index];
}
if (
has_velocity_state_interface_ &&
(has_velocity_command_interface_ || has_effort_command_interface_))
{
error.velocities[index] = desired.velocities[index] - current.velocities[index];
}
if (has_acceleration_state_interface_ && has_acceleration_command_interface_)
{
error.accelerations[index] = desired.accelerations[index] - current.accelerations[index];
}
};

// don't update goal after we sampled the trajectory to avoid any racecondition
const auto active_goal = *rt_active_goal_.readFromRT();

Expand All @@ -177,17 +148,6 @@ controller_interface::return_type JointTrajectoryController::update(
traj_external_point_ptr_->update(*new_external_msg);
}

// TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not
// changed, but its value only?
auto assign_interface_from_point =
[&](auto & joint_interface, const std::vector<double> & trajectory_point_interface)
{
for (size_t index = 0; index < dof_; ++index)
{
joint_interface[index].get().set_value(trajectory_point_interface[index]);
}
};

// current state update
state_current_.time_from_start.set__sec(0);
read_state_from_state_interfaces(state_current_);
Expand Down Expand Up @@ -1216,6 +1176,34 @@ void JointTrajectoryController::goal_accepted_callback(
std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal));
}

void JointTrajectoryController::compute_error_for_joint(
JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current,
const JointTrajectoryPoint & desired) const
{
// error defined as the difference between current and desired
if (joints_angle_wraparound_[index])
{
// if desired, the shortest_angular_distance is calculated, i.e., the error is
// normalized between -pi<error<pi
error.positions[index] =
angles::shortest_angular_distance(current.positions[index], desired.positions[index]);
}
else
{
error.positions[index] = desired.positions[index] - current.positions[index];
}
if (
has_velocity_state_interface_ &&
(has_velocity_command_interface_ || has_effort_command_interface_))
{
error.velocities[index] = desired.velocities[index] - current.velocities[index];
}
if (has_acceleration_state_interface_ && has_acceleration_command_interface_)
{
error.accelerations[index] = desired.accelerations[index] - current.accelerations[index];
}
}

void JointTrajectoryController::fill_partial_goal(
std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg) const
{
Expand Down Expand Up @@ -1277,7 +1265,7 @@ void JointTrajectoryController::fill_partial_goal(
}

void JointTrajectoryController::sort_to_local_joint_order(
std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg)
std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg) const
{
// rearrange all points in the trajectory message based on mapping
std::vector<size_t> mapping_vector = mapping(trajectory_msg->joint_names, params_.joints);
Expand Down
174 changes: 174 additions & 0 deletions joint_trajectory_controller/test/test_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -480,6 +480,180 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup)

// Floating-point value comparison threshold
const double EPS = 1e-6;

/**
* @brief check if calculated trajectory error is correct with angle wraparound=true
*/
TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_true)
{
rclcpp::executors::MultiThreadedExecutor executor;
std::vector<rclcpp::Parameter> params = {};
SetUpAndActivateTrajectoryController(
executor, params, true, 0.0, 1.0, true /* angle_wraparound */);

size_t n_joints = joint_names_.size();

// send msg
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
// *INDENT-OFF*
std::vector<std::vector<double>> points{
{{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
std::vector<std::vector<double>> points_velocities{
{{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
std::vector<std::vector<double>> points_accelerations{
{{0.1, 0.1, 0.1}}, {{0.5, 0.5, 0.5}}, {{0.6, 0.6, 0.6}}};
// *INDENT-ON*

trajectory_msgs::msg::JointTrajectoryPoint error, current, desired;
current.positions = {points[0].begin(), points[0].end()};
current.velocities = {points_velocities[0].begin(), points_velocities[0].end()};
current.accelerations = {points_accelerations[0].begin(), points_accelerations[0].end()};
traj_controller_->resize_joint_trajectory_point(error, n_joints);

// zero error
desired = current;
for (size_t i = 0; i < n_joints; ++i)
{
traj_controller_->testable_compute_error_for_joint(error, i, current, desired);
EXPECT_NEAR(error.positions[i], 0., EPS);
if (
traj_controller_->has_velocity_state_interface() &&
(traj_controller_->has_velocity_command_interface() ||
traj_controller_->has_effort_command_interface()))
{
// expect: error.velocities = desired.velocities - current.velocities;
EXPECT_NEAR(error.velocities[i], 0., EPS);
}
if (
traj_controller_->has_acceleration_state_interface() &&
traj_controller_->has_acceleration_command_interface())
{
// expect: error.accelerations = desired.accelerations - current.accelerations;
EXPECT_NEAR(error.accelerations[i], 0., EPS);
}
}

// angle wraparound of position error
desired.positions[0] += 3.0 * M_PI_2;
desired.velocities[0] += 1.0;
desired.accelerations[0] += 1.0;
traj_controller_->resize_joint_trajectory_point(error, n_joints);
for (size_t i = 0; i < n_joints; ++i)
{
traj_controller_->testable_compute_error_for_joint(error, i, current, desired);
if (i == 0)
{
EXPECT_NEAR(
error.positions[i], desired.positions[i] - current.positions[i] - 2.0 * M_PI, EPS);
}
else
{
EXPECT_NEAR(error.positions[i], desired.positions[i] - current.positions[i], EPS);
}

if (
traj_controller_->has_velocity_state_interface() &&
(traj_controller_->has_velocity_command_interface() ||
traj_controller_->has_effort_command_interface()))
{
// expect: error.velocities = desired.velocities - current.velocities;
EXPECT_NEAR(error.velocities[i], desired.velocities[i] - current.velocities[i], EPS);
}
if (
traj_controller_->has_acceleration_state_interface() &&
traj_controller_->has_acceleration_command_interface())
{
// expect: error.accelerations = desired.accelerations - current.accelerations;
EXPECT_NEAR(error.accelerations[i], desired.accelerations[i] - current.accelerations[i], EPS);
}
}

executor.cancel();
}

/**
* @brief check if calculated trajectory error is correct with angle wraparound=false
*/
TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_false)
{
rclcpp::executors::MultiThreadedExecutor executor;
std::vector<rclcpp::Parameter> params = {};
SetUpAndActivateTrajectoryController(
executor, params, true, 0.0, 1.0, false /* angle_wraparound */);

size_t n_joints = joint_names_.size();

// send msg
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
// *INDENT-OFF*
std::vector<std::vector<double>> points{
{{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
std::vector<std::vector<double>> points_velocities{
{{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
std::vector<std::vector<double>> points_accelerations{
{{0.1, 0.1, 0.1}}, {{0.5, 0.5, 0.5}}, {{0.6, 0.6, 0.6}}};
// *INDENT-ON*

trajectory_msgs::msg::JointTrajectoryPoint error, current, desired;
current.positions = {points[0].begin(), points[0].end()};
current.velocities = {points_velocities[0].begin(), points_velocities[0].end()};
current.accelerations = {points_accelerations[0].begin(), points_accelerations[0].end()};
traj_controller_->resize_joint_trajectory_point(error, n_joints);

// zero error
desired = current;
for (size_t i = 0; i < n_joints; ++i)
{
traj_controller_->testable_compute_error_for_joint(error, i, current, desired);
EXPECT_NEAR(error.positions[i], 0., EPS);
if (
traj_controller_->has_velocity_state_interface() &&
(traj_controller_->has_velocity_command_interface() ||
traj_controller_->has_effort_command_interface()))
{
// expect: error.velocities = desired.velocities - current.velocities;
EXPECT_NEAR(error.velocities[i], 0., EPS);
}
if (
traj_controller_->has_acceleration_state_interface() &&
traj_controller_->has_acceleration_command_interface())
{
// expect: error.accelerations = desired.accelerations - current.accelerations;
EXPECT_NEAR(error.accelerations[i], 0., EPS);
}
}

// no normalization of position error
desired.positions[0] += 3.0 * M_PI_4;
desired.velocities[0] += 1.0;
desired.accelerations[0] += 1.0;
traj_controller_->resize_joint_trajectory_point(error, n_joints);
for (size_t i = 0; i < n_joints; ++i)
{
traj_controller_->testable_compute_error_for_joint(error, i, current, desired);
EXPECT_NEAR(error.positions[i], desired.positions[i] - current.positions[i], EPS);
if (
traj_controller_->has_velocity_state_interface() &&
(traj_controller_->has_velocity_command_interface() ||
traj_controller_->has_effort_command_interface()))
{
// expect: error.velocities = desired.velocities - current.velocities;
EXPECT_NEAR(error.velocities[i], desired.velocities[i] - current.velocities[i], EPS);
}
if (
traj_controller_->has_acceleration_state_interface() &&
traj_controller_->has_acceleration_command_interface())
{
// expect: error.accelerations = desired.accelerations - current.accelerations;
EXPECT_NEAR(error.accelerations[i], desired.accelerations[i] - current.accelerations[i], EPS);
}
}

executor.cancel();
}

/**
* @brief check if position error of revolute joints are wrapped around if not configured so
*/
Expand Down
24 changes: 24 additions & 0 deletions joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,6 +107,13 @@ class TestableJointTrajectoryController

void trigger_declare_parameters() { param_listener_->declare_params(); }

void testable_compute_error_for_joint(
JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current,
const JointTrajectoryPoint & desired)
{
compute_error_for_joint(error, index, current, desired);
}

trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset() const
{
return last_commanded_state_;
Expand Down Expand Up @@ -154,6 +161,23 @@ class TestableJointTrajectoryController
trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; }
trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; }

/**
* a copy of the private member function
*/
void resize_joint_trajectory_point(
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size)
{
point.positions.resize(size, 0.0);
if (has_velocity_state_interface_)
{
point.velocities.resize(size, 0.0);
}
if (has_acceleration_state_interface_)
{
point.accelerations.resize(size, 0.0);
}
}

rclcpp::WaitSet joint_cmd_sub_wait_set_;
};

Expand Down

0 comments on commit 1e634f3

Please sign in to comment.