Skip to content

[JTC] Configurable joint positon error normalization behavior (backport #491) #579

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Apr 23, 2023
Merged

[JTC] Configurable joint positon error normalization behavior (backport #491) #579

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions ...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -166,6 +166,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
std::vector<PidPtr> pids_;
// Feed-forward velocity weight factor when calculating closed loop pid adapter's command
std::vector<double> ff_velocity_scale_;
// Configuration for every joint, if position error is normalized
std::vector<bool> normalize_joint_error_;
// reserved storage for result of the command when closed loop pid adapter is used
std::vector<double> tmp_command_;

Expand Down
24 changes: 20 additions & 4 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,8 +125,17 @@ controller_interface::return_type JointTrajectoryController::update(
const JointTrajectoryPoint & desired)
{
// error defined as the difference between current and desired
error.positions[index] =
angles::shortest_angular_distance(current.positions[index], desired.positions[index]);
if (normalize_joint_error_[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_)
{
error.velocities[index] = desired.velocities[index] - current.velocities[index];
Expand Down Expand Up @@ -243,8 +252,7 @@ controller_interface::return_type JointTrajectoryController::update(
{
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],
state_error_.positions[i], state_error_.velocities[i],
(uint64_t)period.nanoseconds());
}
}
Expand Down Expand Up @@ -581,6 +589,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
}
}

// Configure joint position error normalization from ROS parameters
normalize_joint_error_.resize(dof_);
for (size_t i = 0; i < dof_; ++i)
{
const auto & gains = params_.gains.joints_map.at(params_.joints[i]);
normalize_joint_error_[i] = gains.normalize_error;
}

if (params_.state_interfaces.empty())
{
RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty.");
Expand Down
5 changes: 5 additions & 0 deletions joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -101,6 +101,11 @@ joint_trajectory_controller:
default_value: 0.0,
description: "Feed-forward scaling of velocity."
}
normalize_error: {
type: bool,
default_value: false,
description: "Use position error normalization to -pi to pi."
}
constraints:
stopped_velocity_tolerance: {
type: double,
Expand Down
204 changes: 204 additions & 0 deletions joint_trajectory_controller/test/test_trajectory_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -499,6 +499,210 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency)
EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros);
}

// Floating-point value comparison threshold
const double EPS = 1e-6;
/**
* @brief check if position error of revolute joints are normalized if not configured so
*/
TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized)
{
rclcpp::executors::MultiThreadedExecutor executor;
constexpr double k_p = 10.0;
SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, false);
subscribeToState();

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}}};
// *INDENT-ON*
publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
traj_controller_->wait_for_trajectory(executor);

// first update
updateController(rclcpp::Duration(FIRST_POINT_TIME));

// Spin to receive latest state
executor.spin_some();
auto state_msg = getState();
ASSERT_TRUE(state_msg);

const auto allowed_delta = 0.1;

// no update of state_interface
EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS);

// has the msg the correct vector sizes?
EXPECT_EQ(n_joints, state_msg->desired.positions.size());
EXPECT_EQ(n_joints, state_msg->actual.positions.size());
EXPECT_EQ(n_joints, state_msg->error.positions.size());

// are the correct desired positions used?
EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta);
EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta);
EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta);

// no normalization of position error
EXPECT_NEAR(
state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS);
EXPECT_NEAR(
state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS);
EXPECT_NEAR(
state_msg->error.positions[2], state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2], EPS);

if (traj_controller_->has_position_command_interface())
{
// check command interface
EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta);
EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta);
EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta);
}

if (traj_controller_->has_velocity_command_interface())
{
// check command interface
EXPECT_LE(0.0, joint_vel_[0]);
EXPECT_LE(0.0, joint_vel_[1]);
EXPECT_LE(0.0, joint_vel_[2]);

// use_closed_loop_pid_adapter_
if (traj_controller_->use_closed_loop_pid_adapter())
{
// we expect u = k_p * (s_d-s)
EXPECT_NEAR(
k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
k_p * allowed_delta);
EXPECT_NEAR(
k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
k_p * allowed_delta);
// no normalization of position error
EXPECT_NEAR(
k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2],
k_p * allowed_delta);
}
}

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

executor.cancel();
}

/**
* @brief check if position error of revolute joints are normalized if configured so
*/
TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)
{
rclcpp::executors::MultiThreadedExecutor executor;
constexpr double k_p = 10.0;
SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, true);
subscribeToState();

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}}};
// *INDENT-ON*
publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
traj_controller_->wait_for_trajectory(executor);

// first update
updateController(rclcpp::Duration(FIRST_POINT_TIME));

// Spin to receive latest state
executor.spin_some();
auto state_msg = getState();
ASSERT_TRUE(state_msg);

const auto allowed_delta = 0.1;

// no update of state_interface
EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS);

// has the msg the correct vector sizes?
EXPECT_EQ(n_joints, state_msg->desired.positions.size());
EXPECT_EQ(n_joints, state_msg->actual.positions.size());
EXPECT_EQ(n_joints, state_msg->error.positions.size());

// are the correct desired positions used?
EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta);
EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta);
EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta);

// is error.positions[2] normalized?
EXPECT_NEAR(
state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS);
EXPECT_NEAR(
state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS);
EXPECT_NEAR(
state_msg->error.positions[2],
state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS);

if (traj_controller_->has_position_command_interface())
{
// check command interface
EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta);
EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta);
EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta);
}

if (traj_controller_->has_velocity_command_interface())
{
// check command interface
EXPECT_LE(0.0, joint_vel_[0]);
EXPECT_LE(0.0, joint_vel_[1]);

// use_closed_loop_pid_adapter_
if (traj_controller_->use_closed_loop_pid_adapter())
{
EXPECT_GE(0.0, joint_vel_[2]);
// we expect u = k_p * (s_d-s) for positions[0] and positions[1]
EXPECT_NEAR(
k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
k_p * allowed_delta);
EXPECT_NEAR(
k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
k_p * allowed_delta);
// is error of positions[2] normalized?
EXPECT_NEAR(
k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2],
k_p * allowed_delta);
}
else
{
// interpolated points_velocities only
EXPECT_LE(0.0, joint_vel_[2]);
}
}

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

executor.cancel();
}

void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count)
{
rclcpp::Parameter state_publish_rate_param(
Expand Down
17 changes: 11 additions & 6 deletions joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -112,6 +112,8 @@ class TestableJointTrajectoryController

bool has_effort_command_interface() { return has_effort_command_interface_; }

bool use_closed_loop_pid_adapter() { return use_closed_loop_pid_adapter_; }

rclcpp::WaitSet joint_cmd_sub_wait_set_;
};

Expand Down Expand Up @@ -171,32 +173,35 @@ class TrajectoryControllerTest : public ::testing::Test
node->set_parameters({joint_names_param, cmd_interfaces_params, state_interfaces_params});
}

void SetPidParameters()
void SetPidParameters(
double p_default = 0.0, double ff_default = 1.0, bool normalize_error_default = false)
{
traj_controller_->trigger_declare_parameters();
auto node = traj_controller_->get_node();

for (size_t i = 0; i < joint_names_.size(); ++i)
{
const std::string prefix = "gains." + joint_names_[i];
const rclcpp::Parameter k_p(prefix + ".p", 0.0);
const rclcpp::Parameter k_p(prefix + ".p", p_default);
const rclcpp::Parameter k_i(prefix + ".i", 0.0);
const rclcpp::Parameter k_d(prefix + ".d", 0.0);
const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0);
const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", 1.0);
node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale});
const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_default);
const rclcpp::Parameter normalize_error(prefix + ".normalize_error", normalize_error_default);
node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, normalize_error});
}
}

void SetUpAndActivateTrajectoryController(
rclcpp::Executor & executor, bool use_local_parameters = true,
const std::vector<rclcpp::Parameter> & parameters = {},
bool separate_cmd_and_state_values = false)
bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0,
bool normalize_error = false)
{
SetUpTrajectoryController(executor, use_local_parameters);

// set pid parameters before configure
SetPidParameters();
SetPidParameters(k_p, ff, normalize_error);
for (const auto & param : parameters)
{
traj_controller_->get_node()->set_parameter(param);
Expand Down