Skip to content

Commit 67e5d4d

Browse files
christophfroehlichbmagyar
christophfroehlich
and
bmagyar
authored
[JTC] Configurable joint positon error normalization behavior (#491)
* Use the same variables for state feedback and PID loop * Make joint_error normalization configurable * Activate test for only velocity controller * Allow ff_velocity_scale=0 without deprecated warning * Add test for setpoint due to normalized position error * Update comments in test Co-authored-by: Bence Magyar <[email protected]>
1 parent f617b28 commit 67e5d4d

File tree

5 files changed

+242
-10
lines changed

5 files changed

+242
-10
lines changed

‎joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -166,6 +166,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
166166
std::vector<PidPtr> pids_;
167167
// Feed-forward velocity weight factor when calculating closed loop pid adapter's command
168168
std::vector<double> ff_velocity_scale_;
169+
// Configuration for every joint, if position error is normalized
170+
std::vector<bool> normalize_joint_error_;
169171
// reserved storage for result of the command when closed loop pid adapter is used
170172
std::vector<double> tmp_command_;
171173

‎joint_trajectory_controller/src/joint_trajectory_controller.cpp

Lines changed: 20 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -125,8 +125,17 @@ controller_interface::return_type JointTrajectoryController::update(
125125
const JointTrajectoryPoint & desired)
126126
{
127127
// error defined as the difference between current and desired
128-
error.positions[index] =
129-
angles::shortest_angular_distance(current.positions[index], desired.positions[index]);
128+
if (normalize_joint_error_[index])
129+
{
130+
// if desired, the shortest_angular_distance is calculated, i.e., the error is
131+
// normalized between -pi<error<pi
132+
error.positions[index] =
133+
angles::shortest_angular_distance(current.positions[index], desired.positions[index]);
134+
}
135+
else
136+
{
137+
error.positions[index] = desired.positions[index] - current.positions[index];
138+
}
130139
if (has_velocity_state_interface_ && has_velocity_command_interface_)
131140
{
132141
error.velocities[index] = desired.velocities[index] - current.velocities[index];
@@ -243,8 +252,7 @@ controller_interface::return_type JointTrajectoryController::update(
243252
{
244253
tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) +
245254
pids_[i]->computeCommand(
246-
state_desired_.positions[i] - state_current_.positions[i],
247-
state_desired_.velocities[i] - state_current_.velocities[i],
255+
state_error_.positions[i], state_error_.velocities[i],
248256
(uint64_t)period.nanoseconds());
249257
}
250258
}
@@ -567,6 +575,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
567575
}
568576
}
569577

578+
// Configure joint position error normalization from ROS parameters
579+
normalize_joint_error_.resize(dof_);
580+
for (size_t i = 0; i < dof_; ++i)
581+
{
582+
const auto & gains = params_.gains.joints_map.at(params_.joints[i]);
583+
normalize_joint_error_[i] = gains.normalize_error;
584+
}
585+
570586
if (params_.state_interfaces.empty())
571587
{
572588
RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty.");

‎joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -101,6 +101,11 @@ joint_trajectory_controller:
101101
default_value: 0.0,
102102
description: "Feed-forward scaling of velocity."
103103
}
104+
normalize_error: {
105+
type: bool,
106+
default_value: false,
107+
description: "Use position error normalization to -pi to pi."
108+
}
104109
constraints:
105110
stopped_velocity_tolerance: {
106111
type: double,

‎joint_trajectory_controller/test/test_trajectory_controller.cpp

Lines changed: 204 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -487,6 +487,210 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency)
487487
EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros);
488488
}
489489

490+
// Floating-point value comparison threshold
491+
const double EPS = 1e-6;
492+
/**
493+
* @brief check if position error of revolute joints are normalized if not configured so
494+
*/
495+
TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized)
496+
{
497+
rclcpp::executors::MultiThreadedExecutor executor;
498+
constexpr double k_p = 10.0;
499+
SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, false);
500+
subscribeToState();
501+
502+
size_t n_joints = joint_names_.size();
503+
504+
// send msg
505+
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
506+
builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
507+
// *INDENT-OFF*
508+
std::vector<std::vector<double>> points{
509+
{{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
510+
std::vector<std::vector<double>> points_velocities{
511+
{{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
512+
// *INDENT-ON*
513+
publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
514+
traj_controller_->wait_for_trajectory(executor);
515+
516+
// first update
517+
updateController(rclcpp::Duration(FIRST_POINT_TIME));
518+
519+
// Spin to receive latest state
520+
executor.spin_some();
521+
auto state_msg = getState();
522+
ASSERT_TRUE(state_msg);
523+
524+
const auto allowed_delta = 0.1;
525+
526+
// no update of state_interface
527+
EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS);
528+
529+
// has the msg the correct vector sizes?
530+
EXPECT_EQ(n_joints, state_msg->desired.positions.size());
531+
EXPECT_EQ(n_joints, state_msg->actual.positions.size());
532+
EXPECT_EQ(n_joints, state_msg->error.positions.size());
533+
534+
// are the correct desired positions used?
535+
EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta);
536+
EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta);
537+
EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta);
538+
539+
// no normalization of position error
540+
EXPECT_NEAR(
541+
state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS);
542+
EXPECT_NEAR(
543+
state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS);
544+
EXPECT_NEAR(
545+
state_msg->error.positions[2], state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2], EPS);
546+
547+
if (traj_controller_->has_position_command_interface())
548+
{
549+
// check command interface
550+
EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta);
551+
EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta);
552+
EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta);
553+
}
554+
555+
if (traj_controller_->has_velocity_command_interface())
556+
{
557+
// check command interface
558+
EXPECT_LE(0.0, joint_vel_[0]);
559+
EXPECT_LE(0.0, joint_vel_[1]);
560+
EXPECT_LE(0.0, joint_vel_[2]);
561+
562+
// use_closed_loop_pid_adapter_
563+
if (traj_controller_->use_closed_loop_pid_adapter())
564+
{
565+
// we expect u = k_p * (s_d-s)
566+
EXPECT_NEAR(
567+
k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
568+
k_p * allowed_delta);
569+
EXPECT_NEAR(
570+
k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
571+
k_p * allowed_delta);
572+
// no normalization of position error
573+
EXPECT_NEAR(
574+
k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2],
575+
k_p * allowed_delta);
576+
}
577+
}
578+
579+
if (traj_controller_->has_effort_command_interface())
580+
{
581+
// check command interface
582+
EXPECT_LE(0.0, joint_eff_[0]);
583+
EXPECT_LE(0.0, joint_eff_[1]);
584+
EXPECT_LE(0.0, joint_eff_[2]);
585+
}
586+
587+
executor.cancel();
588+
}
589+
590+
/**
591+
* @brief check if position error of revolute joints are normalized if configured so
592+
*/
593+
TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)
594+
{
595+
rclcpp::executors::MultiThreadedExecutor executor;
596+
constexpr double k_p = 10.0;
597+
SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, true);
598+
subscribeToState();
599+
600+
size_t n_joints = joint_names_.size();
601+
602+
// send msg
603+
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
604+
builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
605+
// *INDENT-OFF*
606+
std::vector<std::vector<double>> points{
607+
{{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
608+
std::vector<std::vector<double>> points_velocities{
609+
{{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
610+
// *INDENT-ON*
611+
publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
612+
traj_controller_->wait_for_trajectory(executor);
613+
614+
// first update
615+
updateController(rclcpp::Duration(FIRST_POINT_TIME));
616+
617+
// Spin to receive latest state
618+
executor.spin_some();
619+
auto state_msg = getState();
620+
ASSERT_TRUE(state_msg);
621+
622+
const auto allowed_delta = 0.1;
623+
624+
// no update of state_interface
625+
EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS);
626+
627+
// has the msg the correct vector sizes?
628+
EXPECT_EQ(n_joints, state_msg->desired.positions.size());
629+
EXPECT_EQ(n_joints, state_msg->actual.positions.size());
630+
EXPECT_EQ(n_joints, state_msg->error.positions.size());
631+
632+
// are the correct desired positions used?
633+
EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta);
634+
EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta);
635+
EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta);
636+
637+
// is error.positions[2] normalized?
638+
EXPECT_NEAR(
639+
state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS);
640+
EXPECT_NEAR(
641+
state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS);
642+
EXPECT_NEAR(
643+
state_msg->error.positions[2],
644+
state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS);
645+
646+
if (traj_controller_->has_position_command_interface())
647+
{
648+
// check command interface
649+
EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta);
650+
EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta);
651+
EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta);
652+
}
653+
654+
if (traj_controller_->has_velocity_command_interface())
655+
{
656+
// check command interface
657+
EXPECT_LE(0.0, joint_vel_[0]);
658+
EXPECT_LE(0.0, joint_vel_[1]);
659+
660+
// use_closed_loop_pid_adapter_
661+
if (traj_controller_->use_closed_loop_pid_adapter())
662+
{
663+
EXPECT_GE(0.0, joint_vel_[2]);
664+
// we expect u = k_p * (s_d-s) for positions[0] and positions[1]
665+
EXPECT_NEAR(
666+
k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
667+
k_p * allowed_delta);
668+
EXPECT_NEAR(
669+
k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
670+
k_p * allowed_delta);
671+
// is error of positions[2] normalized?
672+
EXPECT_NEAR(
673+
k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2],
674+
k_p * allowed_delta);
675+
}
676+
else
677+
{
678+
// interpolated points_velocities only
679+
EXPECT_LE(0.0, joint_vel_[2]);
680+
}
681+
}
682+
683+
if (traj_controller_->has_effort_command_interface())
684+
{
685+
// check command interface
686+
EXPECT_LE(0.0, joint_eff_[0]);
687+
EXPECT_LE(0.0, joint_eff_[1]);
688+
EXPECT_LE(0.0, joint_eff_[2]);
689+
}
690+
691+
executor.cancel();
692+
}
693+
490694
void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count)
491695
{
492696
rclcpp::Parameter state_publish_rate_param(

‎joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

Lines changed: 11 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -112,6 +112,8 @@ class TestableJointTrajectoryController
112112

113113
bool has_effort_command_interface() { return has_effort_command_interface_; }
114114

115+
bool use_closed_loop_pid_adapter() { return use_closed_loop_pid_adapter_; }
116+
115117
rclcpp::WaitSet joint_cmd_sub_wait_set_;
116118
};
117119

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

174-
void SetPidParameters()
176+
void SetPidParameters(
177+
double p_default = 0.0, double ff_default = 1.0, bool normalize_error_default = false)
175178
{
176179
traj_controller_->trigger_declare_parameters();
177180
auto node = traj_controller_->get_node();
178181

179182
for (size_t i = 0; i < joint_names_.size(); ++i)
180183
{
181184
const std::string prefix = "gains." + joint_names_[i];
182-
const rclcpp::Parameter k_p(prefix + ".p", 0.0);
185+
const rclcpp::Parameter k_p(prefix + ".p", p_default);
183186
const rclcpp::Parameter k_i(prefix + ".i", 0.0);
184187
const rclcpp::Parameter k_d(prefix + ".d", 0.0);
185188
const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0);
186-
const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", 1.0);
187-
node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale});
189+
const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_default);
190+
const rclcpp::Parameter normalize_error(prefix + ".normalize_error", normalize_error_default);
191+
node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, normalize_error});
188192
}
189193
}
190194

191195
void SetUpAndActivateTrajectoryController(
192196
rclcpp::Executor & executor, bool use_local_parameters = true,
193197
const std::vector<rclcpp::Parameter> & parameters = {},
194-
bool separate_cmd_and_state_values = false)
198+
bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0,
199+
bool normalize_error = false)
195200
{
196201
SetUpTrajectoryController(executor, use_local_parameters);
197202

198203
// set pid parameters before configure
199-
SetPidParameters();
204+
SetPidParameters(k_p, ff, normalize_error);
200205
for (const auto & param : parameters)
201206
{
202207
traj_controller_->get_node()->set_parameter(param);

0 commit comments

Comments
 (0)