Skip to content

Commit

Permalink
[JTC] Angle wraparound for first segment of trajectory (#796)
Browse files Browse the repository at this point in the history
(cherry picked from commit e0491ba)
  • Loading branch information
christophfroehlich committed Mar 27, 2024
1 parent 333beb0 commit 6187f83
Show file tree
Hide file tree
Showing 6 changed files with 148 additions and 65 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -154,7 +154,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 wrapped around
// Configuration for every joint if it wraps around (ie. is continuous, position error is
// normalized)
std::vector<bool> joints_angle_wraparound_;
// reserved storage for result of the command when closed loop pid adapter is used
std::vector<double> tmp_command_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -44,14 +44,19 @@ class Trajectory
const trajectory_msgs::msg::JointTrajectoryPoint & current_point,
std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory);

/// Set the point before the trajectory message is replaced/appended
/// Example: if we receive a new trajectory message and it's first point is 0.5 seconds
/// from the current one, we call this function to log the current state, then
/// append/replace the current trajectory
/**
* Set the point before the trajectory message is replaced/appended
* Example: if we receive a new trajectory message and it's first point is 0.5 seconds
* from the current one, we call this function to log the current state, then
* append/replace the current trajectory
* \param joints_angle_wraparound Vector of boolean where true value corresponds to a joint that
* wrap around (ie. is continuous).
*/
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
void set_point_before_trajectory_msg(
const rclcpp::Time & current_time,
const trajectory_msgs::msg::JointTrajectoryPoint & current_point);
const trajectory_msgs::msg::JointTrajectoryPoint & current_point,
const std::vector<bool> & joints_angle_wraparound = std::vector<bool>());

JOINT_TRAJECTORY_CONTROLLER_PUBLIC
void update(std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory);
Expand Down Expand Up @@ -189,6 +194,17 @@ inline std::vector<size_t> mapping(const T & t1, const T & t2)
return mapping_vector;
}

/**
* \param current_position The current position given from the controller, which will be adapted.
* \param next_position Next position from which to compute the wraparound offset, i.e.,
* the first trajectory point
* \param joints_angle_wraparound Vector of boolean where true value corresponds to a joint that
* wrap around (ie. is continuous).
*/
void wraparound_joint(
std::vector<double> & current_position, const std::vector<double> next_position,
const std::vector<bool> & joints_angle_wraparound);

} // namespace joint_trajectory_controller

#endif // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -205,11 +205,13 @@ controller_interface::return_type JointTrajectoryController::update(
first_sample = true;
if (params_.open_loop_control)
{
traj_external_point_ptr_->set_point_before_trajectory_msg(time, last_commanded_state_);
traj_external_point_ptr_->set_point_before_trajectory_msg(
time, last_commanded_state_, joints_angle_wraparound_);
}
else
{
traj_external_point_ptr_->set_point_before_trajectory_msg(time, state_current_);
traj_external_point_ptr_->set_point_before_trajectory_msg(
time, state_current_, joints_angle_wraparound_);
}
}

Expand Down
32 changes: 31 additions & 1 deletion joint_trajectory_controller/src/trajectory.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@

#include <memory>

#include "angles/angles.h"
#include "hardware_interface/macros.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/time.hpp"
Expand Down Expand Up @@ -44,10 +45,39 @@ Trajectory::Trajectory(

void Trajectory::set_point_before_trajectory_msg(
const rclcpp::Time & current_time,
const trajectory_msgs::msg::JointTrajectoryPoint & current_point)
const trajectory_msgs::msg::JointTrajectoryPoint & current_point,
const std::vector<bool> & joints_angle_wraparound)
{
time_before_traj_msg_ = current_time;
state_before_traj_msg_ = current_point;

// Compute offsets due to wrapping joints
wraparound_joint(
state_before_traj_msg_.positions, trajectory_msg_->points[0].positions,
joints_angle_wraparound);
}

void wraparound_joint(
std::vector<double> & current_position, const std::vector<double> next_position,
const std::vector<bool> & joints_angle_wraparound)
{
double dist;
// joints_angle_wraparound is even empty, or has the same size as the number of joints
for (size_t i = 0; i < joints_angle_wraparound.size(); i++)
{
if (joints_angle_wraparound[i])
{
dist = angles::shortest_angular_distance(current_position[i], next_position[i]);

// Deal with singularity at M_PI shortest distance
if (std::abs(std::abs(dist) - M_PI) < 1e-9)
{
dist = next_position[i] > current_position[i] ? std::abs(dist) : -std::abs(dist);
}

current_position[i] = next_position[i] - dist;
}
}
}

void Trajectory::update(std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory)
Expand Down
56 changes: 56 additions & 0 deletions joint_trajectory_controller/test/test_trajectory.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -821,3 +821,59 @@ TEST(TestTrajectory, skip_interpolation)
}
}
}

TEST(TestWrapAroundJoint, no_wraparound)
{
const std::vector<double> initial_position(3, 0.);
std::vector<double> next_position(3, M_PI * 3. / 2.);

std::vector<double> current_position(initial_position);
std::vector<bool> joints_angle_wraparound(3, false);
joint_trajectory_controller::wraparound_joint(
current_position, next_position, joints_angle_wraparound);
EXPECT_EQ(current_position[0], initial_position[0]);
EXPECT_EQ(current_position[1], initial_position[1]);
EXPECT_EQ(current_position[2], initial_position[2]);
}

TEST(TestWrapAroundJoint, wraparound_single_joint)
{
const std::vector<double> initial_position(3, 0.);
std::vector<double> next_position(3, M_PI * 3. / 2.);

std::vector<double> current_position(initial_position);
std::vector<bool> joints_angle_wraparound{true, false, false};
joint_trajectory_controller::wraparound_joint(
current_position, next_position, joints_angle_wraparound);
EXPECT_EQ(current_position[0], initial_position[0] + 2 * M_PI);
EXPECT_EQ(current_position[1], initial_position[1]);
EXPECT_EQ(current_position[2], initial_position[2]);
}

TEST(TestWrapAroundJoint, wraparound_all_joints)
{
const std::vector<double> initial_position(3, 0.);
std::vector<double> next_position(3, M_PI * 3. / 2.);

std::vector<double> current_position(initial_position);
std::vector<bool> joints_angle_wraparound(3, true);
joint_trajectory_controller::wraparound_joint(
current_position, next_position, joints_angle_wraparound);
EXPECT_EQ(current_position[0], initial_position[0] + 2 * M_PI);
EXPECT_EQ(current_position[1], initial_position[1] + 2 * M_PI);
EXPECT_EQ(current_position[2], initial_position[2] + 2 * M_PI);
}

TEST(TestWrapAroundJoint, wraparound_all_joints_no_offset)
{
const std::vector<double> initial_position(3, 0.);
std::vector<double> next_position(3, M_PI * 3. / 2.);

std::vector<double> current_position(next_position);
std::vector<bool> joints_angle_wraparound(3, true);
joint_trajectory_controller::wraparound_joint(
current_position, next_position, joints_angle_wraparound);
EXPECT_EQ(current_position[0], next_position[0]);
EXPECT_EQ(current_position[1], next_position[1]);
EXPECT_EQ(current_position[2], next_position[2]);
}
90 changes: 34 additions & 56 deletions joint_trajectory_controller/test/test_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -514,14 +514,16 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup)
// Floating-point value comparison threshold
const double EPS = 1e-6;
/**
* @brief check if position error of revolute joints are angle_wraparound if not configured so
* @brief check if position error of revolute joints are wrapped around if not configured so
*/
TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparound)
{
rclcpp::executors::MultiThreadedExecutor executor;
constexpr double k_p = 10.0;
std::vector<rclcpp::Parameter> params = {};
SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false);
bool angle_wraparound = false;
SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, angle_wraparound);
subscribeToState();

size_t n_joints = joint_names_.size();

Expand All @@ -531,10 +533,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun
// *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);
publish(time_from_start, points, rclcpp::Time());
traj_controller_->wait_for_trajectory(executor);

updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME));
Expand Down Expand Up @@ -598,35 +598,24 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun

if (traj_controller_->has_effort_command_interface())
{
// use_closed_loop_pid_adapter_
if (traj_controller_->use_closed_loop_pid_adapter())
{
// we expect u = k_p * (s_d-s) for positions
EXPECT_NEAR(
k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0],
k_p * COMMON_THRESHOLD);
EXPECT_NEAR(
k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1],
k_p * COMMON_THRESHOLD);
EXPECT_NEAR(
k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2],
k_p * COMMON_THRESHOLD);
}
else
{
// interpolated points_velocities only
// check command interface
EXPECT_LT(0.0, joint_eff_[0]);
EXPECT_LT(0.0, joint_eff_[1]);
EXPECT_LT(0.0, joint_eff_[2]);
}
// with effort command interface, use_closed_loop_pid_adapter is always true
// we expect u = k_p * (s_d-s) for positions
EXPECT_NEAR(
k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0],
k_p * COMMON_THRESHOLD);
EXPECT_NEAR(
k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1],
k_p * COMMON_THRESHOLD);
EXPECT_NEAR(
k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2],
k_p * COMMON_THRESHOLD);
}

executor.cancel();
}

/**
* @brief check if position error of revolute joints are angle_wraparound if configured so
* @brief check if position error of revolute joints are wrapped around if configured so
*/
TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
{
Expand Down Expand Up @@ -669,7 +658,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD);
EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD);

// is error.positions[2] angle_wraparound?
// is error.positions[2] wrapped around?
EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS);
EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS);
EXPECT_NEAR(
Expand All @@ -688,15 +677,15 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
// use_closed_loop_pid_adapter_
if (traj_controller_->use_closed_loop_pid_adapter())
{
// we expect u = k_p * (s_d-s) for positions[0] and positions[1]
// we expect u = k_p * (s_d-s) for joint0 and joint1
EXPECT_NEAR(
k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
k_p * COMMON_THRESHOLD);
EXPECT_NEAR(
k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
k_p * COMMON_THRESHOLD);
// is error of positions[2] angle_wraparound?
EXPECT_GT(0.0, joint_vel_[2]);
// is error of positions[2] wrapped around?
EXPECT_GT(0.0, joint_vel_[2]); // direction change because of angle wrap
EXPECT_NEAR(
k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2],
k_p * COMMON_THRESHOLD);
Expand All @@ -713,30 +702,19 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)

if (traj_controller_->has_effort_command_interface())
{
// use_closed_loop_pid_adapter_
if (traj_controller_->use_closed_loop_pid_adapter())
{
// we expect u = k_p * (s_d-s) for positions[0] and positions[1]
EXPECT_NEAR(
k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0],
k_p * COMMON_THRESHOLD);
EXPECT_NEAR(
k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1],
k_p * COMMON_THRESHOLD);
// is error of positions[2] angle_wraparound?
EXPECT_GT(0.0, joint_eff_[2]);
EXPECT_NEAR(
k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2],
k_p * COMMON_THRESHOLD);
}
else
{
// interpolated points_velocities only
// check command interface
EXPECT_LT(0.0, joint_eff_[0]);
EXPECT_LT(0.0, joint_eff_[1]);
EXPECT_LT(0.0, joint_eff_[2]);
}
// with effort command interface, use_closed_loop_pid_adapter is always true
// we expect u = k_p * (s_d-s) for joint0 and joint1
EXPECT_NEAR(
k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0],
k_p * COMMON_THRESHOLD);
EXPECT_NEAR(
k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1],
k_p * COMMON_THRESHOLD);
// is error of positions[2] wrapped around?
EXPECT_GT(0.0, joint_eff_[2]);
EXPECT_NEAR(
k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2],
k_p * COMMON_THRESHOLD);
}

executor.cancel();
Expand Down

0 comments on commit 6187f83

Please sign in to comment.