diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index 468201f11..a62d6e729 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -71,21 +71,17 @@ enum CommandInterfaces point to the hardware interface. 2.0: The hardware interface will read the point written from the controller. The state will switch between 1.0 and 2.0 until all points have been read by the hardware interface. - 3.0: The hardware interface has read all the points, and will now write all the points to the robot driver. - 4.0: The robot is moving through the trajectory. - 5.0: The robot finished executing the trajectory. */ + 3.0: The hardware interface has read all the points, and will now write all the points to the physical robot + controller. 4.0: The robot is moving through the trajectory. 5.0: The robot finished executing the trajectory. */ PASSTHROUGH_TRAJECTORY_TRANSFER_STATE = 0, - /* The PASSTHROUGH_TRAJECTORY_CANCEL value is used to indicate whether the trajectory has been cancelled from the + /* The PASSTHROUGH_TRAJECTORY_ABORT value is used to indicate whether the trajectory has been cancelled from the * hardware interface.*/ - PASSTHROUGH_TRAJECTORY_CANCEL = 1, - /* The PASSTHROUGH_TRAJECTORY_DIMENSIONS is used to indicate what combination of joint positions, velocities and - * accelerations the trajectory consists of, see check_dimensions() for a description of what the values mean. */ - PASSTHROUGH_TRAJECTORY_DIMENSIONS = 2, + PASSTHROUGH_TRAJECTORY_ABORT = 1, /* Arrays to hold the values of a point. */ - PASSTHROUGH_TRAJECTORY_POSITIONS_ = 3, - PASSTHROUGH_TRAJECTORY_VELOCITIES_ = 9, - PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ = 15, - PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 21 + PASSTHROUGH_TRAJECTORY_POSITIONS_ = 2, + PASSTHROUGH_TRAJECTORY_VELOCITIES_ = 8, + PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ = 14, + PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 20 }; enum StateInterfaces @@ -93,8 +89,7 @@ enum StateInterfaces /* State interface 0 - 17 are joint state interfaces*/ SPEED_SCALING_FACTOR = 18, - NUMBER_OF_JOINTS = 19, - CONTROLLER_RUNNING = 20 + CONTROLLER_RUNNING = 19 }; class PassthroughTrajectoryController : public controller_interface::ControllerInterface @@ -138,26 +133,15 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI void goal_accepted_callback( const std::shared_ptr> goal_handle); - void execute( - const std::shared_ptr> goal_handle); - std::vector joint_names_; std::vector state_interface_types_; std::vector> joint_position_state_interface_; std::vector> joint_velocity_state_interface_; - /* - If there are values in the velocities and accelerations vectors, they should have as many elements as there are - joints, and all be the same size. - The return value indicates what combination of joint positions, velocities and accelerations is present; - 0: The trajectory is invalid, and will be rejected. - 1: Only positions are defined for the trajectory. - 2: Positions and velocities are defined for the trajectory. - 3: Positions and accelerations are defined for the trajectory. - 4: Both positions, velocities and accelerations are defined for the trajectory. - */ - int check_dimensions(std::shared_ptr goal); + bool check_positions(std::shared_ptr goal); + bool check_velocities(std::shared_ptr goal); + bool check_accelerations(std::shared_ptr goal); trajectory_msgs::msg::JointTrajectory active_joint_traj_; std::vector path_tolerance_; @@ -174,6 +158,7 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI double scaling_factor_; uint32_t number_of_joints_; std::shared_ptr> active_goal_; + static constexpr double NO_VAL = std::numeric_limits::quiet_NaN(); }; } // namespace ur_controllers #endif // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 092d6aa5e..4514fb113 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -87,7 +87,6 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::st } } conf.names.emplace_back(passthrough_params_.speed_scaling_interface_name); - conf.names.emplace_back(tf_prefix + "passthrough_controller/number_of_joints"); conf.names.emplace_back(tf_prefix + "passthrough_controller/running"); return conf; @@ -102,9 +101,7 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_transfer_state"); - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_cancel"); - - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_dimensions"); + config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_abort"); for (size_t i = 0; i < 6; ++i) { config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_positions_" + @@ -145,6 +142,85 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Time& time, const rclcpp::Duration& /* period */) { + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != 0.0) { + /* Check if the trajectory has been aborted from the hardware interface. E.g. the robot was stopped on the teach + * pendant. */ + if (command_interfaces_[PASSTHROUGH_TRAJECTORY_ABORT].get_value() == 1.0) { + RCLCPP_INFO(get_node()->get_logger(), "Trajectory cancelled from hardware interface, aborting action."); + std::shared_ptr result = + std::make_shared(); + active_goal_->abort(result); + end_goal(); + return controller_interface::return_type::OK; + } + /* Check if the goal has been cancelled by the ROS user. */ + if (active_goal_->is_canceling()) { + std::shared_ptr result = + std::make_shared(); + active_goal_->canceled(result); + end_goal(); + return controller_interface::return_type::OK; + } + // Write a new point to the command interface, if the previous point has been read by the hardware interface. + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 1.0) { + if (current_point_ < active_joint_traj_.points.size()) { + // Write the time_from_start parameter. + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( + active_joint_traj_.points[current_point_].time_from_start.sec + + (active_joint_traj_.points[current_point_].time_from_start.nanosec / 1000000000)); + // Write the positions for each joint of the robot + for (int i = 0; i < 6; i++) { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value( + active_joint_traj_.points[current_point_].positions[i]); + // Optionally, also write velocities and accelerations for each joint. + if (active_joint_traj_.points[current_point_].velocities.size() > 0) { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_VELOCITIES_ + i].set_value( + active_joint_traj_.points[current_point_].velocities[i]); + } else { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_VELOCITIES_].set_value(NO_VAL); + } + + if (active_joint_traj_.points[current_point_].accelerations.size() > 0) { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ + i].set_value( + active_joint_traj_.points[current_point_].accelerations[i]); + } else { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ + i].set_value(NO_VAL); + } + } + // Tell hardware interface that this point is ready to be read. + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(2.0); + current_point_++; + + // Check if all points have been written to the hardware interface. + } else if (current_point_ == active_joint_traj_.points.size()) { + RCLCPP_INFO(get_node()->get_logger(), "All points sent to the hardware interface, trajectory will now " + "execute!"); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(3.0); + } + // When the trajectory is finished, report the goal as successful to the client. + } else if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 5.0) { + std::shared_ptr result = + std::make_shared(); + // Check if the actual position complies with the tolerances given. + if (!check_goal_tolerance()) { + result->error_code = control_msgs::action::FollowJointTrajectory::Result::GOAL_TOLERANCE_VIOLATED; + result->error_string = "Robot not within tolerances at end of trajectory."; + active_goal_->abort(result); + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory failed, goal tolerances not met."); + // Check if the goal time tolerance was complied with. + } else if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + goal_time_tolerance_.seconds())) { + result->error_code = control_msgs::action::FollowJointTrajectory::Result::GOAL_TOLERANCE_VIOLATED; + result->error_string = "Goal not reached within time tolerance"; + active_goal_->abort(result); + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory failed, goal time tolerance not met."); + } else { + result->error_code = control_msgs::action::FollowJointTrajectory::Result::SUCCESSFUL; + active_goal_->succeed(result); + RCLCPP_INFO(get_node()->get_logger(), "Trajectory executed successfully!"); + } + end_goal(); + } + } static bool firstpass = true; if (firstpass) { now_ns = time.nanoseconds(); @@ -190,71 +266,102 @@ rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callb return rclcpp_action::GoalResponse::REJECT; } - // Check dimensions of the trajectory - if (check_dimensions(goal) == 0) { - RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected, dimensions of trajectory are incorrect."); + // Check that all parts of the trajectory are valid. + if (!check_positions(goal)) { + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected"); + return rclcpp_action::GoalResponse::REJECT; + } + if (!check_velocities(goal)) { + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected"); + return rclcpp_action::GoalResponse::REJECT; + } + if (!check_accelerations(goal)) { + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected"); return rclcpp_action::GoalResponse::REJECT; - } else { - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_DIMENSIONS].set_value( - static_cast(check_dimensions(goal))); } return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } -int PassthroughTrajectoryController::check_dimensions( +bool PassthroughTrajectoryController::check_positions( std::shared_ptr goal) { for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) { - std::string msg; if (goal->trajectory.points[i].positions.size() != number_of_joints_) { + std::string msg; msg = "Can't accept new trajectory. All trajectory points must have positions for all joints of the robot. (" + std::to_string(number_of_joints_) + " joint positions per point)"; RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); msg = "Point nr " + std::to_string(i + 1) + " has: " + std::to_string(goal->trajectory.points[i].positions.size()) + " positions."; RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); - return 0; + return false; } - if ((goal->trajectory.points[i].velocities.size() != 0 && - goal->trajectory.points[i].velocities.size() != number_of_joints_) || - goal->trajectory.points[i].velocities.size() != goal->trajectory.points[0].velocities.size()) { - msg = "Can't accept new trajectory. All trajectory points must have velocities for all joints of the robot. (" + + } + return true; +} + +bool PassthroughTrajectoryController::check_velocities( + std::shared_ptr goal) +{ + for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) { + if (goal->trajectory.points[i].velocities.size() != number_of_joints_ && + goal->trajectory.points[i].velocities.size() != 0) { + std::string msg; + msg = "Can't accept new trajectory. All trajectory points must either not have velocities or have them for all " + "joints of the robot. (" + std::to_string(number_of_joints_) + " joint velocities per point)"; RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); msg = "Point nr " + std::to_string(i + 1) + " has: " + std::to_string(goal->trajectory.points[i].velocities.size()) + " velocities."; RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); - return 0; + return false; } - if ((goal->trajectory.points[i].accelerations.size() != 0 && - goal->trajectory.points[i].accelerations.size() != number_of_joints_) || - goal->trajectory.points[i].accelerations.size() != goal->trajectory.points[0].accelerations.size()) { - msg = "Can't accept new trajectory. All trajectory points must have accelerations for all joints of the robot. " + if (goal->trajectory.points[i].velocities.size() != goal->trajectory.points[0].velocities.size()) { + std::string msg; + msg = "Can't accept new trajectory. All trajectory points must have velocities for all joints of the robot. " "(" + + std::to_string(number_of_joints_) + " joint velocities per point)"; + RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); + msg = "Point nr " + std::to_string(i) + " has: " + std::to_string(goal->trajectory.points[i].velocities.size()) + + " velocities."; + RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); + return false; + } + } + return true; +} + +bool PassthroughTrajectoryController::check_accelerations( + std::shared_ptr goal) +{ + for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) { + if (goal->trajectory.points[i].accelerations.size() != 0 && + goal->trajectory.points[i].accelerations.size() != number_of_joints_) { + std::string msg; + msg = "Can't accept new trajectory. All trajectory points must either not have accelerations or have them for " + "all joints of the robot. (" + std::to_string(number_of_joints_) + " joint accelerations per point)"; RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); - msg = "Point nr " + std::to_string(i + 1) + + msg = "Point nr " + std::to_string(i) + " has: " + std::to_string(goal->trajectory.points[i].accelerations.size()) + " accelerations."; RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); - return 0; + return false; + } + if (goal->trajectory.points[i].accelerations.size() != goal->trajectory.points[0].accelerations.size()) { + std::string msg; + msg = "Can't accept new trajectory. All trajectory points must have accelerations for all joints of the " + "robot. " + "(" + + std::to_string(number_of_joints_) + " joint accelerations per point)"; + RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); + msg = "Point nr " + std::to_string(i) + + " has: " + std::to_string(goal->trajectory.points[i].accelerations.size()) + " accelerations."; + RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); + return false; } } - if (goal->trajectory.points[0].velocities.size() == 6 && goal->trajectory.points[0].accelerations.size() == 0) { - /*If there are only positions and velocities defined */ - return 2; - } else if (goal->trajectory.points[0].velocities.size() == 0 && - goal->trajectory.points[0].accelerations.size() == 6) { - /*If there are only positions and accelerations defined */ - return 3; - } else if (goal->trajectory.points[0].velocities.size() == 6 && - goal->trajectory.points[0].accelerations.size() == 6) { - /*If there are both positions, velocities and accelerations defined */ - return 4; - } else { - /*If there are only positions defined */ - return 1; - } + return true; } // Called when the action is cancelled by the action client. @@ -275,7 +382,7 @@ void PassthroughTrajectoryController::goal_accepted_callback( active_trajectory_elapsed_time_ = 0; current_point_ = 0; - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_CANCEL].set_value(0.0); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_ABORT].set_value(0.0); active_joint_traj_ = goal_handle->get_goal()->trajectory; goal_tolerance_ = goal_handle->get_goal()->goal_tolerance; @@ -288,95 +395,6 @@ void PassthroughTrajectoryController::goal_accepted_callback( command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(1.0); - /* Start the executing thread of the action server, this will run until the trajectory is finished or cancelled. */ - std::thread{ std::bind(&PassthroughTrajectoryController::execute, this, std::placeholders::_1), goal_handle } - .detach(); - return; -} - -void PassthroughTrajectoryController::execute( - const std::shared_ptr> goal_handle) -{ - /* While loop should execute 200 times pr second. Can be made slower if needed, but it will affect how fast the points - * of the trajectory are transferred to the hardware interface. */ - rclcpp::Rate loop_rate(200); - while (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != 0.0) { - /* Check if the trajectory has been cancelled from the hardware interface. E.g. the robot was stopped on the teach - * pendant. */ - if (command_interfaces_[PASSTHROUGH_TRAJECTORY_CANCEL].get_value() == 1.0) { - RCLCPP_INFO(get_node()->get_logger(), "Trajectory cancelled from hardware interface, aborting action."); - std::shared_ptr result = - std::make_shared(); - goal_handle->abort(result); - end_goal(); - return; - } - /* Check if the goal has been cancelled by the ROS user. */ - if (goal_handle->is_canceling()) { - std::shared_ptr result = - std::make_shared(); - goal_handle->canceled(result); - end_goal(); - return; - } - // Write a new point to the command interface, if the previous point has been read by the hardware interface. - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 1.0) { - if (current_point_ < active_joint_traj_.points.size()) { - // Write the time_from_start parameter. - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( - active_joint_traj_.points[current_point_].time_from_start.sec + - (active_joint_traj_.points[current_point_].time_from_start.nanosec / 1000000000)); - // Write the positions for each joint of the robot - for (int i = 0; i < 6; i++) { - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value( - active_joint_traj_.points[current_point_].positions[i]); - // Optionally, also write velocities and accelerations for each joint. - if (active_joint_traj_.points[current_point_].velocities.size() > 0) { - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_VELOCITIES_ + i].set_value( - active_joint_traj_.points[current_point_].velocities[i]); - } - - if (active_joint_traj_.points[current_point_].accelerations.size() > 0) { - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ + i].set_value( - active_joint_traj_.points[current_point_].accelerations[i]); - } - } - // Tell hardware interface that this point is ready to be read. - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(2.0); - current_point_++; - - // Check if all points have been written to the hardware interface. - } else if (current_point_ == active_joint_traj_.points.size()) { - RCLCPP_INFO(get_node()->get_logger(), "All points sent to the hardware interface, trajectory will now " - "execute!"); - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(3.0); - } - // When the trajectory is finished, report the goal as successful to the client. - } else if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 5.0) { - std::shared_ptr result = - std::make_shared(); - // Check if the actual position complies with the tolerances given. - if (!check_goal_tolerance()) { - result->error_code = control_msgs::action::FollowJointTrajectory::Result::GOAL_TOLERANCE_VIOLATED; - result->error_string = "Robot not within tolerances at end of trajectory."; - goal_handle->abort(result); - RCLCPP_ERROR(get_node()->get_logger(), "Trajectory failed, goal tolerances not met."); - // Check if the goal time tolerance was complied with. - } else if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + goal_time_tolerance_.seconds())) { - result->error_code = control_msgs::action::FollowJointTrajectory::Result::GOAL_TOLERANCE_VIOLATED; - result->error_string = "Goal not reached within time tolerance"; - goal_handle->abort(result); - RCLCPP_ERROR(get_node()->get_logger(), "Trajectory failed, goal time tolerance not met."); - } else { - result->error_code = control_msgs::action::FollowJointTrajectory::Result::SUCCESSFUL; - goal_handle->succeed(result); - RCLCPP_INFO(get_node()->get_logger(), "Trajectory executed successfully!"); - } - end_goal(); - return; - } - loop_rate.sleep(); - } return; } diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 94179be03..d007e0383 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -138,6 +138,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface void transformForceTorque(); void check_passthrough_trajectory_controller(); void trajectory_done_callback(urcl::control::TrajectoryResult result); + bool has_accelerations(std::vector> accelerations); + bool has_velocities(std::vector> velocities); urcl::vector6d_t urcl_position_commands_; urcl::vector6d_t urcl_position_commands_old_; @@ -197,15 +199,12 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface // Passthrough trajectory controller interface values double passthrough_trajectory_transfer_state_; - double passthrough_trajectory_cancel_; - double passthrough_trajectory_dimensions_; + double passthrough_trajectory_abort_; double passthrough_trajectory_controller_running_; urcl::vector6d_t passthrough_trajectory_positions_; urcl::vector6d_t passthrough_trajectory_velocities_; urcl::vector6d_t passthrough_trajectory_accelerations_; double passthrough_trajectory_time_from_start_; - double number_of_joints_; - // payload stuff urcl::vector3d_t payload_center_of_gravity_; double payload_mass_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 3ff309955..c36c83f95 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -95,11 +95,10 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys async_thread_shutdown_ = false; system_interface_initialized_ = 0.0; passthrough_trajectory_transfer_state_ = 0.0; - passthrough_trajectory_cancel_ = 0.0; + passthrough_trajectory_abort_ = 0.0; trajectory_joint_positions_.clear(); trajectory_joint_velocities_.clear(); trajectory_joint_accelerations_.clear(); - number_of_joints_ = info_.joints.size(); for (const hardware_interface::ComponentInfo& joint : info_.joints) { if (joint.command_interfaces.size() != 2) { @@ -237,9 +236,6 @@ std::vector URPositionHardwareInterface::exp state_interfaces.emplace_back( hardware_interface::StateInterface(tf_prefix + "gpio", "program_running", &robot_program_running_copy_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(tf_prefix + "passthrough_controller", "number_of_joints", &number_of_joints_)); - state_interfaces.emplace_back(hardware_interface::StateInterface(tf_prefix + "passthrough_controller", "running", &passthrough_trajectory_controller_running_)); @@ -315,10 +311,7 @@ std::vector URPositionHardwareInterface::e &passthrough_trajectory_transfer_state_)); command_interfaces.emplace_back(hardware_interface::CommandInterface( - tf_prefix + "passthrough_controller", "passthrough_trajectory_cancel", &passthrough_trajectory_cancel_)); - - command_interfaces.emplace_back(hardware_interface::CommandInterface( - tf_prefix + "passthrough_controller", "passthrough_trajectory_dimensions", &passthrough_trajectory_dimensions_)); + tf_prefix + "passthrough_controller", "passthrough_trajectory_abort", &passthrough_trajectory_abort_)); for (size_t i = 0; i < 6; ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( @@ -919,7 +912,7 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod } else if (stop_modes_.size() != 0 && std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_PASSTHROUGH) != stop_modes_.end()) { passthrough_trajectory_controller_running_ = 0.0; - passthrough_trajectory_cancel_ = 1.0; + passthrough_trajectory_abort_ = 1.0; trajectory_joint_positions_.clear(); trajectory_joint_accelerations_.clear(); trajectory_joint_velocities_.clear(); @@ -962,36 +955,35 @@ void URPositionHardwareInterface::check_passthrough_trajectory_controller() trajectory_times_.push_back(passthrough_trajectory_time_from_start_ - last_time); last_time = passthrough_trajectory_time_from_start_; - if (passthrough_trajectory_dimensions_ == 2.0 || passthrough_trajectory_dimensions_ == 4.0) { + if (!std::isnan(passthrough_trajectory_velocities_[0])) { trajectory_joint_velocities_.push_back(passthrough_trajectory_velocities_); } - if (passthrough_trajectory_dimensions_ == 3.0 || passthrough_trajectory_dimensions_ == 4.0) { + if (!std::isnan(passthrough_trajectory_accelerations_[0])) { trajectory_joint_accelerations_.push_back(passthrough_trajectory_accelerations_); } passthrough_trajectory_transfer_state_ = 1.0; - /* When all points have been read, write them to the robot driver.*/ + /* When all points have been read, write them to the physical robot controller.*/ } else if (passthrough_trajectory_transfer_state_ == 3.0) { - /* Tell robot driver how many points are in the trajectory. */ + /* Tell robot controller how many points are in the trajectory. */ ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, trajectory_joint_positions_.size()); - /* Write the appropriate type of point according to the dimensions of the trajectory. See - * passthrough_trajectory_controller.hpp - check_dimensions() for an explanation of the values. */ - if (passthrough_trajectory_dimensions_ == 1.0) { + /* Write the appropriate type of point depending on the combination of positions, velocities and accelerations. */ + if (!has_velocities(trajectory_joint_velocities_) && !has_accelerations(trajectory_joint_accelerations_)) { for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], urcl::vector6d_t{ 0, 0, 0, 0, 0, 0 }, trajectory_times_[i]); } - } else if (passthrough_trajectory_dimensions_ == 2.0) { + } else if (has_velocities(trajectory_joint_velocities_) && !has_accelerations(trajectory_joint_accelerations_)) { for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], trajectory_joint_velocities_[i], trajectory_times_[i]); } - } else if (passthrough_trajectory_dimensions_ == 3.0) { + } else if (!has_velocities(trajectory_joint_velocities_) && has_accelerations(trajectory_joint_accelerations_)) { for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], trajectory_joint_accelerations_[i], trajectory_times_[i]); } - } else if (passthrough_trajectory_dimensions_ == 4.0) { + } else if (has_velocities(trajectory_joint_velocities_) && has_accelerations(trajectory_joint_accelerations_)) { for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], trajectory_joint_velocities_[i], trajectory_joint_accelerations_[i], trajectory_times_[i]); @@ -1000,6 +992,8 @@ void URPositionHardwareInterface::check_passthrough_trajectory_controller() trajectory_joint_positions_.clear(); trajectory_joint_accelerations_.clear(); trajectory_joint_velocities_.clear(); + trajectory_times_.clear(); + last_time = 0.0; passthrough_trajectory_transfer_state_ = 4.0; } } @@ -1009,11 +1003,21 @@ void URPositionHardwareInterface::trajectory_done_callback(urcl::control::Trajec if (result == urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS) { passthrough_trajectory_transfer_state_ = 5.0; } else { - passthrough_trajectory_cancel_ = 1.0; + passthrough_trajectory_abort_ = 1.0; } return; } +bool URPositionHardwareInterface::has_velocities(std::vector> velocities) +{ + return (velocities.size() > 0); +} + +bool URPositionHardwareInterface::has_accelerations(std::vector> accelerations) +{ + return (accelerations.size() > 0); +} + } // namespace ur_robot_driver #include "pluginlib/class_list_macros.hpp"