From 1e8f13f2323f2c8f2608b75a763ba906671275dd Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 24 Sep 2015 13:50:53 +0200 Subject: [PATCH] Added control switching --- README.md | 2 - config/ur10_controllers.yaml | 25 ++++++---- config/ur5_controllers.yaml | 12 ++--- .../ur_modern_driver/ur_hardware_interface.h | 4 +- launch/ur5_ros_control.launch | 4 +- src/ur_hardware_interface.cpp | 47 ++++++++++++++++++- src/ur_ros_wrapper.cpp | 4 +- 7 files changed, 73 insertions(+), 25 deletions(-) diff --git a/README.md b/README.md index 7e6ad23..136e208 100644 --- a/README.md +++ b/README.md @@ -41,8 +41,6 @@ Besides this, the driver subscribes to two new topics: */joint\_speed : takes messages of type trajectory\_msgs/JointTrajectory, parses the first JointTracetoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order. -This driver is written in c++, which should make it possible to integrate it with ros_control. If you feel like undertaking this task, please dive right in. I don't have the posibility to do this. - No script is sent to the robot. This means that the teach pendant can be used to move the robot around while the driver is running. --- diff --git a/config/ur10_controllers.yaml b/config/ur10_controllers.yaml index 1389f4d..54c62be 100644 --- a/config/ur10_controllers.yaml +++ b/config/ur10_controllers.yaml @@ -33,7 +33,7 @@ position_trajectory_controller: - wrist_1_joint - wrist_2_joint - wrist_3_joint - constraints: + constraints: goal_time: 0.6 stopped_velocity_tolerance: 0.05 shoulder_pan_joint: {trajectory: 0.1, goal: 0.1} @@ -42,14 +42,19 @@ position_trajectory_controller: wrist_1_joint: {trajectory: 0.1, goal: 0.1} wrist_2_joint: {trajectory: 0.1, goal: 0.1} wrist_3_joint: {trajectory: 0.1, goal: 0.1} - stop_trajectory_duration: 0.5 - state_publish_rate: 125 - action_monitor_rate: 10 - # gains: - # joint1: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} - # joint2: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + stop_trajectory_duration: 0.5 + state_publish_rate: 125 + action_monitor_rate: 10 + gains: + #!!These values have not been optimized!! + shoulder_pan_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + shoulder_lift_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + elbow_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_1_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_2_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_3_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - # state_publish_rate: 50 # Defaults to 50 - # action_monitor_rate: 20 # Defaults to 20 - #hold_trajectory_duration: 0 # Defaults to 0.5 + # state_publish_rate: 50 # Defaults to 50 + # action_monitor_rate: 20 # Defaults to 20 + #hold_trajectory_duration: 0 # Defaults to 0.5 diff --git a/config/ur5_controllers.yaml b/config/ur5_controllers.yaml index 0f629ea..54c62be 100644 --- a/config/ur5_controllers.yaml +++ b/config/ur5_controllers.yaml @@ -47,12 +47,12 @@ position_trajectory_controller: action_monitor_rate: 10 gains: #!!These values have not been optimized!! - shoulder_pan_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} - shoulder_lift_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} - elbow_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} - wrist_1_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} - wrist_2_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} - wrist_3_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + shoulder_pan_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + shoulder_lift_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + elbow_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_1_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_2_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_3_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} # state_publish_rate: 50 # Defaults to 50 # action_monitor_rate: 20 # Defaults to 20 diff --git a/include/ur_modern_driver/ur_hardware_interface.h b/include/ur_modern_driver/ur_hardware_interface.h index 707ad53..5720811 100644 --- a/include/ur_modern_driver/ur_hardware_interface.h +++ b/include/ur_modern_driver/ur_hardware_interface.h @@ -86,6 +86,8 @@ public: /// \brief write the command to the robot hardware. virtual void write(); + void doSwitch(const std::list&, const std::list&); + protected: // Startup and shutdown of the internal node inside a roscpp program @@ -95,7 +97,7 @@ protected: hardware_interface::JointStateInterface joint_state_interface_; hardware_interface::ForceTorqueSensorInterface force_torque_interface_; hardware_interface::VelocityJointInterface velocity_joint_interface_; - + bool velocity_interface_running_; // Shared memory std::vector joint_names_; std::vector joint_position_; diff --git a/launch/ur5_ros_control.launch b/launch/ur5_ros_control.launch index ca35ba3..050553b 100644 --- a/launch/ur5_ros_control.launch +++ b/launch/ur5_ros_control.launch @@ -28,8 +28,8 @@ - + diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp index 9705af4..e5cf596 100644 --- a/src/ur_hardware_interface.cpp +++ b/src/ur_hardware_interface.cpp @@ -106,6 +106,7 @@ void UrHardwareInterface::init() { registerInterface(&joint_state_interface_); // From RobotHW base class. registerInterface(&velocity_joint_interface_); // From RobotHW base class. registerInterface(&force_torque_interface_); // From RobotHW base class. + velocity_interface_running_ = false; } void UrHardwareInterface::read() { @@ -127,7 +128,51 @@ void UrHardwareInterface::read() { } void UrHardwareInterface::write() { - robot_->setSpeed(joint_velocity_command_[0],joint_velocity_command_[1],joint_velocity_command_[2],joint_velocity_command_[3],joint_velocity_command_[4],joint_velocity_command_[5],100); + if (velocity_interface_running_) { + robot_->setSpeed(joint_velocity_command_[0], joint_velocity_command_[1], + joint_velocity_command_[2], joint_velocity_command_[3], + joint_velocity_command_[4], joint_velocity_command_[5], 100); + } +} + +void UrHardwareInterface::doSwitch( + const std::list& start_list, + const std::list& stop_list) { + for (std::list::const_iterator controller_it = + start_list.begin(); controller_it != start_list.end(); + ++controller_it) { + if (controller_it->hardware_interface + == "hardware_interface::VelocityJointInterface") { + velocity_interface_running_ = true; + ROS_DEBUG("Starting velocity interface"); + } + } + for (std::list::const_iterator controller_it = + stop_list.begin(); controller_it != stop_list.end(); + ++controller_it) { + if (controller_it->hardware_interface + == "hardware_interface::VelocityJointInterface") { + velocity_interface_running_ = false; + ROS_DEBUG("Stopping velocity interface"); + } + } + /*std::string outp; + outp = "doSwitch called - Start list:"; + for (std::list::const_iterator controller_it = + start_list.begin(); controller_it != start_list.end(); + ++controller_it) { + outp += " " + controller_it->name + "(" + controller_it->hardware_interface + ")"; + } + outp += " - Stop list:"; + for (std::list::const_iterator controller_it = + stop_list.begin(); controller_it != stop_list.end(); + ++controller_it) { + outp += " " + controller_it->name + "(" + controller_it->hardware_interface + ")"; + + } + ROS_INFO(outp.c_str()); */ + } } // namespace + diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 08edc72..3c0d382 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -363,14 +363,12 @@ private: last_time = current_time; // Input hardware_interface_->read(); - + robot_.rt_interface_->robot_state_->setControllerUpdated(); // Control - //controller_manager_->update(ros::Time::now(), elapsed_time); controller_manager_->update(ros::Time(current_time.tv_sec, current_time.tv_nsec), elapsed_time); // Output hardware_interface_->write(); - robot_.rt_interface_->robot_state_->setControllerUpdated(); } }