From b7f0ce209511dc7e4beac4806e9a7cc244d45c7f Mon Sep 17 00:00:00 2001 From: "Stein M. Nornes" Date: Fri, 25 Mar 2022 15:02:39 +0100 Subject: [PATCH] JointTrajectory: Don't use shortest_angular_distance for linear joints Fixes https://github.com/ros-controls/ros_controllers/issues/432 --- .../joint_trajectory_controller.h | 1 + .../joint_trajectory_controller_impl.h | 17 ++++++++++++++++- 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h index cf1bce375..5b68668e6 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h @@ -182,6 +182,7 @@ class JointTrajectoryController : public controller_interface::Controller joints_; ///< Handles to controlled joints. std::vector angle_wraparound_; ///< Whether controlled joints wrap around or not. + std::vector is_linear_; ///< Whether controlled joints are linear or not. std::vector joint_names_; ///< Controlled joint names. SegmentTolerances default_tolerances_; ///< Default trajectory segment tolerances. HwIfaceAdapter hw_iface_adapter_; ///< Adapts desired trajectory state to HW interface. diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h index 58be9e348..7fd7cfb33 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h @@ -254,6 +254,7 @@ bool JointTrajectoryController::init(HardwareInt // Initialize members joints_.resize(n_joints); angle_wraparound_.resize(n_joints); + is_linear_.resize(n_joints); for (unsigned int i = 0; i < n_joints; ++i) { // Joint handle @@ -267,13 +268,16 @@ bool JointTrajectoryController::init(HardwareInt // Whether a joint is continuous (ie. has angle wraparound) angle_wraparound_[i] = urdf_joints[i]->type == urdf::Joint::CONTINUOUS; + is_linear_[i] = urdf_joints[i]->type == urdf::Joint::PRISMATIC; const std::string not_if = angle_wraparound_[i] ? "" : "non-"; + const std::string linear = is_linear_[i] ? "linear " : ""; - ROS_DEBUG_STREAM_NAMED(name_, "Found " << not_if << "continuous joint '" << joint_names_[i] << "' in '" << + ROS_DEBUG_STREAM_NAMED(name_, "Found " << not_if << "continuous " << linear << "joint '" << joint_names_[i] << "' in '" << this->getHardwareInterfaceType() << "'."); } assert(joints_.size() == angle_wraparound_.size()); + assert(joints_.size() == is_linear_.size()); ROS_DEBUG_STREAM_NAMED(name_, "Initialized controller '" << name_ << "' with:" << "\n- Number of joints: " << getNumberOfJoints() << "\n- Hardware interface type: '" << this->getHardwareInterfaceType() << "'" << @@ -779,6 +783,17 @@ updateStates(const ros::Time& sample_time, const Trajectory* const traj) desired_state_.velocity[joint_index] = desired_joint_state_.velocity[0]; desired_state_.acceleration[joint_index] = desired_joint_state_.acceleration[0]; + if (is_linear_[joint_index]) + { + state_joint_error_.position[0] = desired_joint_state_.position[0] - current_state_.position[joint_index]; + } + else + { + state_joint_error_.position[0] = angles::shortest_angular_distance(current_state_.position[joint_index],desired_joint_state_.position[0]); + } + state_joint_error_.velocity[0] = desired_joint_state_.velocity[0] - current_state_.velocity[joint_index]; + state_joint_error_.acceleration[0] = 0.0; + state_error_.position[joint_index] = angles::shortest_angular_distance(current_state_.position[joint_index],desired_joint_state_.position[0]); state_error_.velocity[joint_index] = desired_joint_state_.velocity[0] - current_state_.velocity[joint_index]; state_error_.acceleration[joint_index] = 0.0;