Skip to content

Commit

Permalink
[JTC] Implement effort-only command interface (#225)
Browse files Browse the repository at this point in the history 
* Fix trajectory tolerance parameters
* Implement effort command interface for JTC
* Use auto_declare for pid params
* Set effort to 0 on deactivate

Co-authored-by: Denis Štogl <[email protected]>
(cherry picked from commit 97c9431)

# Conflicts:
#	joint_trajectory_controller/src/joint_trajectory_controller.cpp
#	joint_trajectory_controller/test/test_trajectory_controller.cpp
#	joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
  • Loading branch information
@Ace314159 @mergify
Ace314159 authored and mergify[bot] committed May 3, 2022
1 parent d0a27c3 commit 04b0ced
Show file tree
Hide file tree
Showing 4 changed files with 250 additions and 19 deletions.
4 changes: 1 addition & 3 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 @@ -139,11 +139,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
bool has_position_command_interface_ = false;
bool has_velocity_command_interface_ = false;
bool has_acceleration_command_interface_ = false;
bool has_effort_command_interface_ = false;

/// If true, a velocity feedforward term plus corrective PID term is used
// TODO(anyone): This flag is not used for now
// There should be PID-approach used as in ROS1:
// https://github.com/ros-controls/ros_controllers/blob/noetic-devel/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h#L283
bool use_closed_loop_pid_adapter = false;

// TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported
Expand Down
145 changes: 133 additions & 12 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -191,23 +191,52 @@ controller_interface::return_type JointTrajectoryController::update(
const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end();

// set values for next hardware write()
if (use_closed_loop_pid_adapter)
{
// Update PIDs
for (auto i = 0ul; i < joint_num; ++i)
{
tmp_command_[i] = (state_desired.velocities[i] * ff_velocity_scale_[i]) +
pids_[i]->computeCommand(
state_desired.positions[i] - state_current.positions[i],
state_desired.velocities[i] - state_current.velocities[i],
(uint64_t)period.nanoseconds());
}
}
if (has_position_command_interface_)
{
assign_interface_from_point(joint_command_interface_[0], state_desired.positions);
}
if (has_velocity_command_interface_)
{
<<<<<<< HEAD
assign_interface_from_point(joint_command_interface_[1], state_desired.velocities);
=======
if (use_closed_loop_pid_adapter)
{
assign_interface_from_point(joint_command_interface_[1], tmp_command_);
}
else
{
assign_interface_from_point(joint_command_interface_[1], state_desired.velocities);
}
>>>>>>> 97c9431 ([JTC] Implement effort-only command interface (#225))
}
if (has_acceleration_command_interface_)
{
assign_interface_from_point(joint_command_interface_[2], state_desired.accelerations);
}
// TODO(anyone): Add here "if using_closed_loop_hw_interface_adapter" (see ROS1) - #171
// if (check_if_interface_type_exist(
// command_interface_types_, hardware_interface::HW_IF_EFFORT)) {
// assign_interface_from_point(joint_command_interface_[3], state_desired.effort);
// }
if (has_effort_command_interface_)
{
if (use_closed_loop_pid_adapter)
{
assign_interface_from_point(joint_command_interface_[3], tmp_command_);
}
else
{
assign_interface_from_point(joint_command_interface_[3], state_desired.effort);
}
}

for (size_t index = 0; index < joint_num; ++index)
{
Expand Down Expand Up @@ -463,6 +492,7 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S
// 2. velocity
// 2. position [velocity, [acceleration]]

<<<<<<< HEAD
// effort can't be combined with other interfaces
if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_EFFORT))
{
Expand Down Expand Up @@ -495,6 +525,16 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S
{
has_acceleration_command_interface_ = true;
}
=======
has_position_command_interface_ =
contains_interface_type(command_interface_types_, hardware_interface::HW_IF_POSITION);
has_velocity_command_interface_ =
contains_interface_type(command_interface_types_, hardware_interface::HW_IF_VELOCITY);
has_acceleration_command_interface_ =
contains_interface_type(command_interface_types_, hardware_interface::HW_IF_ACCELERATION);
has_effort_command_interface_ =
contains_interface_type(command_interface_types_, hardware_interface::HW_IF_EFFORT);
>>>>>>> 97c9431 ([JTC] Implement effort-only command interface (#225))

if (has_velocity_command_interface_)
{
Expand Down Expand Up @@ -526,6 +566,44 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S
return CallbackReturn::FAILURE;
}

<<<<<<< HEAD
=======
// effort can't be combined with other interfaces
if (has_effort_command_interface_)
{
if (command_interface_types_.size() == 1)
{
use_closed_loop_pid_adapter = true;
}
else
{
RCLCPP_ERROR(logger, "'effort' command interface has to be used alone.");
return CallbackReturn::FAILURE;
}
}

if (use_closed_loop_pid_adapter)
{
size_t num_joints = joint_names_.size();
pids_.resize(num_joints);
ff_velocity_scale_.resize(num_joints);
tmp_command_.resize(num_joints, 0.0);

// Init PID gains from ROS parameter server
for (size_t i = 0; i < pids_.size(); ++i)
{
const std::string prefix = "gains." + joint_names_[i];
const auto k_p = auto_declare<double>(prefix + ".p", 0.0);
const auto k_i = auto_declare<double>(prefix + ".i", 0.0);
const auto k_d = auto_declare<double>(prefix + ".d", 0.0);
const auto i_clamp = auto_declare<double>(prefix + ".i_clamp", 0.0);
ff_velocity_scale_[i] = auto_declare<double>("ff_velocity_scale/" + joint_names_[i], 0.0);
// Initialize PID
pids_[i] = std::make_shared<control_toolbox::Pid>(k_p, k_i, k_d, i_clamp, -i_clamp);
}
}

>>>>>>> 97c9431 ([JTC] Implement effort-only command interface (#225))
// Read always state interfaces from the parameter because they can be used
// independently from the controller's type.
// Specialized, child controllers should set its default value.
Expand Down Expand Up @@ -569,7 +647,7 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S

if (has_velocity_state_interface_)
{
if (!contains_interface_type(state_interface_types_, hardware_interface::HW_IF_POSITION))
if (!has_position_state_interface_)
{
RCLCPP_ERROR(
logger,
Expand All @@ -578,13 +656,33 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S
return CallbackReturn::FAILURE;
}
}
else if (has_acceleration_state_interface_)
else
{
RCLCPP_ERROR(
logger,
"'acceleration' state interface cannot be used if 'position' and 'velocity' "
"interfaces are not present.");
return CallbackReturn::FAILURE;
if (has_acceleration_state_interface_)
{
RCLCPP_ERROR(
logger,
"'acceleration' state interface cannot be used if 'position' and 'velocity' "
"interfaces are not present.");
return CallbackReturn::FAILURE;
}
if (has_velocity_command_interface_ && command_interface_types_.size() == 1)
{
RCLCPP_ERROR(
logger,
"'velocity' command interface can only be used alone if 'velocity' and "
"'position' state interfaces are present");
return CallbackReturn::FAILURE;
}
// effort is always used alone so no need for size check
if (has_effort_command_interface_)
{
RCLCPP_ERROR(
logger,
"'effort' command interface can only be used alone if 'velocity' and "
"'position' state interfaces are present");
return CallbackReturn::FAILURE;
}
}

auto get_interface_list = [](const std::vector<std::string> & interface_types) {
Expand Down Expand Up @@ -774,8 +872,26 @@ CallbackReturn JointTrajectoryController::on_deactivate(const rclcpp_lifecycle::
// TODO(anyone): How to halt when using effort commands?
for (size_t index = 0; index < joint_names_.size(); ++index)
{
<<<<<<< HEAD
joint_command_interface_[0][index].get().set_value(
joint_command_interface_[0][index].get().get_value());
=======
if (has_position_command_interface_)
{
joint_command_interface_[0][index].get().set_value(
joint_command_interface_[0][index].get().get_value());
}

if (has_velocity_command_interface_)
{
joint_command_interface_[1][index].get().set_value(0.0);
}

if (has_effort_command_interface_)
{
joint_command_interface_[3][index].get().set_value(0.0);
}
>>>>>>> 97c9431 ([JTC] Implement effort-only command interface (#225))
}

for (size_t index = 0; index < allowed_interface_types_.size(); ++index)
Expand Down Expand Up @@ -813,6 +929,11 @@ bool JointTrajectoryController::reset()
subscriber_is_active_ = false;
joint_command_subscriber_.reset();

for (const auto & pid : pids_)
{
pid->reset();
}

// iterator has no default value
// prev_traj_point_ptr_;
traj_point_active_ptr_ = nullptr;
Expand Down
102 changes: 98 additions & 4 deletions joint_trajectory_controller/test/test_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -309,6 +309,13 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta);
EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta);

if (traj_controller_->has_effort_command_interface())
{
EXPECT_LE(0.0, joint_eff_[0]);
EXPECT_LE(0.0, joint_eff_[1]);
EXPECT_LE(0.0, joint_eff_[2]);
}

// cleanup
state = traj_controller_->cleanup();

Expand Down Expand Up @@ -447,6 +454,25 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order)
traj_msg.points[0].positions[1] = 3.0;
traj_msg.points[0].positions[2] = 1.0;

<<<<<<< HEAD
=======
if (traj_controller_->has_velocity_command_interface())
{
traj_msg.points[0].velocities.resize(3);
traj_msg.points[0].velocities[0] = -0.1;
traj_msg.points[0].velocities[1] = -0.1;
traj_msg.points[0].velocities[2] = -0.1;
}

if (traj_controller_->has_effort_command_interface())
{
traj_msg.points[0].effort.resize(3);
traj_msg.points[0].effort[0] = -0.1;
traj_msg.points[0].effort[1] = -0.1;
traj_msg.points[0].effort[2] = -0.1;
}

>>>>>>> 97c9431 ([JTC] Implement effort-only command interface (#225))
trajectory_publisher_->publish(traj_msg);
}

Expand All @@ -456,9 +482,32 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order)
// Currently COMMON_THRESHOLD is adjusted.
updateController(rclcpp::Duration::from_seconds(0.25));

<<<<<<< HEAD
EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD);
EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD);
EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD);
=======
if (traj_controller_->has_position_command_interface())
{
EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD);
EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD);
EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD);
}

if (traj_controller_->has_velocity_command_interface())
{
EXPECT_GT(0.0, joint_vel_[0]);
EXPECT_GT(0.0, joint_vel_[1]);
EXPECT_GT(0.0, joint_vel_[2]);
}

if (traj_controller_->has_effort_command_interface())
{
EXPECT_GT(0.0, joint_eff_[0]);
EXPECT_GT(0.0, joint_eff_[1]);
EXPECT_GT(0.0, joint_eff_[2]);
}
>>>>>>> 97c9431 ([JTC] Implement effort-only command interface (#225))
}

/**
Expand Down Expand Up @@ -511,6 +560,18 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list)
EXPECT_NEAR(0.0, joint_vel_[2], threshold)
<< "Joint 3 velocity should be 0.0 since it's not in the goal";
}

if (
std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") !=
command_interface_types_.end())
{
// estimate the sign of the velocity
// joint rotates forward
EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0]));
EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1]));
EXPECT_NEAR(0.0, joint_eff_[2], threshold)
<< "Joint 3 effort should be 0.0 since it's not in the goal";
}
// TODO(anyone): add here ckecks for acceleration commands

executor.cancel();
Expand Down Expand Up @@ -1104,6 +1165,29 @@ INSTANTIATE_TEST_SUITE_P(
std::vector<std::string>({"position", "velocity", "acceleration"}),
std::vector<std::string>({"position", "velocity", "acceleration"}))));

<<<<<<< HEAD
=======
// only velocity controller
INSTANTIATE_TEST_SUITE_P(
OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized,
::testing::Values(
std::make_tuple(
std::vector<std::string>({"velocity"}), std::vector<std::string>({"position", "velocity"})),
std::make_tuple(
std::vector<std::string>({"velocity"}),
std::vector<std::string>({"position", "velocity", "acceleration"}))));

// only effort controller
INSTANTIATE_TEST_CASE_P(
OnlyEffortTrajectoryControllers, TrajectoryControllerTestParameterized,
::testing::Values(
std::make_tuple(
std::vector<std::string>({"effort"}), std::vector<std::string>({"position", "velocity"})),
std::make_tuple(
std::vector<std::string>({"effort"}),
std::vector<std::string>({"position", "velocity", "acceleration"}))));

>>>>>>> 97c9431 ([JTC] Implement effort-only command interface (#225))
TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters)
{
auto set_parameter_and_check_result = [&]() {
Expand All @@ -1123,10 +1207,6 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame
command_interface_types_ = {"bad_name"};
set_parameter_and_check_result();

// command interfaces: effort not yet implemented
command_interface_types_ = {"effort"};
set_parameter_and_check_result();

// command interfaces: effort has to be only
command_interface_types_ = {"effort", "position"};
set_parameter_and_check_result();
Expand Down Expand Up @@ -1164,4 +1244,18 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame
// state interfaces: acceleration without position and velocity
state_interface_types_ = {"acceleration"};
set_parameter_and_check_result();

// velocity-only command interface: position - velocity not present
command_interface_types_ = {"velocity"};
state_interface_types_ = {"position"};
set_parameter_and_check_result();
state_interface_types_ = {"velocity"};
set_parameter_and_check_result();

// effort-only command interface: position - velocity not present
command_interface_types_ = {"effort"};
state_interface_types_ = {"position"};
set_parameter_and_check_result();
state_interface_types_ = {"velocity"};
set_parameter_and_check_result();
}
Loading

0 comments on commit 04b0ced

Please sign in to comment.