Skip to content
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.

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

[JTC] Angle wraparound for first segment of trajectory #796

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -155,7 +155,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 @@ -153,11 +153,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 @@ -636,14 +636,16 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_fal
}

/**
* @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 @@ -653,10 +655,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 @@ -720,35 +720,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 @@ -791,7 +780,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 @@ -810,15 +799,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 @@ -835,30 +824,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
Loading