diff --git a/CMakeLists.txt b/CMakeLists.txt index 1a9702e..cfd2a37 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -126,7 +126,7 @@ if(COMPILER_SUPPORTS_CXX11) elseif(COMPILER_SUPPORTS_CXX0X) set(CMAKE_CXX_FLAGS "-std=c++0x") else() - message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") + message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler. Suggested solution: update the pkg build-essential ") endif() ## Specify additional locations of header files @@ -174,29 +174,19 @@ target_link_libraries(ur_driver ## Install ## ############# -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) ## Mark executables and/or libraries for installation -# install(TARGETS ur_modern_driver ur_driver -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +install(TARGETS ur_driver ur_hardware_interface + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) ## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) ## Mark other files for installation (e.g. launch and bag files, etc.) # install(FILES diff --git a/LICENSE b/LICENSE index b68de61..35548b1 100644 --- a/LICENSE +++ b/LICENSE @@ -1,7 +1,180 @@ -"THE BEER-WARE LICENSE" (Revision 42): +Copyright 2015 Thomas Timm Andersen - wrote this software. As long as you retain this notice you -can do whatever you want with this stuff. If we meet some day, and you think -this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + diff --git a/README.md b/README.md index 8748d54..62946e8 100644 --- a/README.md +++ b/README.md @@ -28,9 +28,13 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design * Added support for ros_control. * As ros_control wants to have control over the robot at all times, ros_control compatability is set via a parameter at launch-time. * With ros_control active, the driver doesn't open the action_lib interface nor publish joint_states or wrench msgs. This is handled by ros_control instead. + * Currently two controllers are available, both controlling the joint position of the robot, useable for trajectroy execution + * The velocity based controller sends joint speed commands to the robot, using the speej command + * The position based controller send joint position commands to the robot, using the servoj command + * I have so far only used the velocity based controller, but which one is optimal depends on the application. * As ros_control continuesly controls the robot, using the teach pendant while a controller is running will cause the controller **on the robot** to crash, as it obviously can't handle conflicting control input from two sources. Thus be sure to stop the running controller **before** moving the robot via the teach pendant: * A list of the loaded and running controllers can be found by a call to the controller_manager ```rosservice call /controller_manager/list_controllers {} ``` - * The running position trajectory controller can be stopped with a call to ```rosservice call /universal_robot/controller_manager/switch_controller "start_controllers: - '' stop_controllers: - 'position_based_position_trajectory_controller' strictness: 1" ``` (Remember you can use tab-completion for this) + * The running position trajectory controller can be stopped with a call to ```rosservice call /universal_robot/controller_manager/switch_controller "start_controllers: - '' stop_controllers: - 'pos_based_pos_traj_controller' strictness: 1" ``` (Remember you can use tab-completion for this) ## Installation @@ -61,12 +65,12 @@ Be sure to stop the currently running controller **either before or in the same The position based controller *should* stay closer to the commanded path, while the velocity based react faster (trajectory execution start within 50-70 ms, while it is in the 150-180ms range for the position_based. Usage without ros_control as well as the old driver is also in the 170ms range, as mentioned at my lightning talk @ ROSCon 2013). -**Note** that the PID values are not tweaked as of this moment. +**Note** that the PID values are not optimally tweaked as of this moment. To use ros_control together with MoveIt, be sure to add the desired controller to the ```controllers.yaml``` in the urXX_moveit_config/config folder. Add the following ``` controller_list: - - name: velocity_based_position_trajectory_controller #or position_based_position_trajectory_controller + - name: vel_based_pos_traj_controller #or pos_based_pos_traj_controller action_ns: follow_joint_trajectory type: FollowJointTrajectory default: true @@ -100,3 +104,16 @@ Should be compatible with all robots and control boxes with the newest firmware. *Simulated UR5 running 1.6.08725 +#Credits +Please cite the following report if using this driver + +@techreport{andersen2015optimizing, + title = {Optimizing the Universal Robots ROS driver.}, + institution = {Technical University of Denmark, Department of Electrical Engineering}, + author = {Andersen, Thomas Timm}, + year = {2015}, + url = {http://orbit.dtu.dk/en/publications/optimizing-the-universal-robots-ros-driver(20dde139-7e87-4552-8658-dbf2cdaab24b).html} + } + + +The report can be downloaded from http://orbit.dtu.dk/en/publications/optimizing-the-universal-robots-ros-driver(20dde139-7e87-4552-8658-dbf2cdaab24b).html diff --git a/config/ur10_controllers.yaml b/config/ur10_controllers.yaml index dd679bc..19e0394 100644 --- a/config/ur10_controllers.yaml +++ b/config/ur10_controllers.yaml @@ -22,10 +22,10 @@ force_torque_sensor_controller: type: force_torque_sensor_controller/ForceTorqueSensorController publish_rate: 125 -# Joint Trajectory Controller ------------------------------- +# Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -velocity_based_position_trajectory_controller: - type: velocity_controllers/JointTrajectoryController +pos_based_pos_traj_controller: + type: position_controllers/JointTrajectoryController joints: - shoulder_pan_joint - shoulder_lift_joint @@ -47,12 +47,49 @@ velocity_based_position_trajectory_controller: 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} + 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} + + # state_publish_rate: 50 # Defaults to 50 + # action_monitor_rate: 20 # Defaults to 20 + #hold_trajectory_duration: 0 # Defaults to 0.5 + +# Joint Trajectory Controller ------------------------------- +# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller +vel_based_pos_traj_controller: + type: velocity_controllers/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + shoulder_pan_joint: {trajectory: 0.1, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.1, goal: 0.1} + elbow_joint: {trajectory: 0.1, goal: 0.1} + 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: + #!!These values are useable, but maybe not optimal + shoulder_pan_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + shoulder_lift_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + elbow_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_1_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_2_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_3_joint: {p: 20.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/config/ur3_controllers.yaml b/config/ur3_controllers.yaml new file mode 100644 index 0000000..8e26689 --- /dev/null +++ b/config/ur3_controllers.yaml @@ -0,0 +1,97 @@ +# Currently simply a copy of ur5_controllers.yaml + +# Settings for ros_control control loop +hardware_control_loop: + loop_hz: 125 + +# Settings for ros_control hardware interface +hardware_interface: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + +# Publish all joint states ---------------------------------- +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 125 + +# Publish wrench ---------------------------------- +force_torque_sensor_controller: + type: force_torque_sensor_controller/ForceTorqueSensorController + publish_rate: 125 + +# Joint Trajectory Controller - position based ------------------------------- +# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller +position_based_position_trajectory_controller: + type: position_controllers/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + shoulder_pan_joint: {trajectory: 0.1, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.1, goal: 0.1} + elbow_joint: {trajectory: 0.1, goal: 0.1} + 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: + #!!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} + + # state_publish_rate: 50 # Defaults to 50 + # action_monitor_rate: 20 # Defaults to 20 + #hold_trajectory_duration: 0 # Defaults to 0.5 + +# Joint Trajectory Controller - velocity based ------------------------------- +# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller +velocity_based_position_trajectory_controller: + type: velocity_controllers/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + shoulder_pan_joint: {trajectory: 0.1, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.1, goal: 0.1} + elbow_joint: {trajectory: 0.1, goal: 0.1} + 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: + #!!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 diff --git a/config/ur5_controllers.yaml b/config/ur5_controllers.yaml index 57591ca..4eab52e 100644 --- a/config/ur5_controllers.yaml +++ b/config/ur5_controllers.yaml @@ -24,7 +24,7 @@ force_torque_sensor_controller: # Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -position_based_position_trajectory_controller: +pos_based_pos_traj_controller: type: position_controllers/JointTrajectoryController joints: - shoulder_pan_joint @@ -60,7 +60,7 @@ position_based_position_trajectory_controller: # Joint Trajectory Controller - velocity based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -velocity_based_position_trajectory_controller: +vel_based_pos_traj_controller: type: velocity_controllers/JointTrajectoryController joints: - shoulder_pan_joint @@ -82,13 +82,13 @@ velocity_based_position_trajectory_controller: 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} + #!!These values are useable, but maybe not optimal + shoulder_pan_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + shoulder_lift_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + elbow_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_1_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_2_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_3_joint: {p: 20.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/do_output.h b/include/ur_modern_driver/do_output.h index 8031821..d52420e 100644 --- a/include/ur_modern_driver/do_output.h +++ b/include/ur_modern_driver/do_output.h @@ -1,12 +1,19 @@ /* * do_output.h * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #ifndef UR_DO_OUTPUT_H_ diff --git a/include/ur_modern_driver/robot_state.h b/include/ur_modern_driver/robot_state.h index 2a2b077..159974b 100644 --- a/include/ur_modern_driver/robot_state.h +++ b/include/ur_modern_driver/robot_state.h @@ -1,8 +1,19 @@ /* * robot_state.h * - * Created on: Sep 10, 2015 - * Author: ttan + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #ifndef ROBOT_STATE_H_ @@ -54,6 +65,38 @@ enum robot_message_type { } typedef robot_message_types::robot_message_type robotMessageType; +namespace robot_state_type_v18 { +enum robot_state_type { + ROBOT_RUNNING_MODE = 0, + ROBOT_FREEDRIVE_MODE = 1, + ROBOT_READY_MODE = 2, + ROBOT_INITIALIZING_MODE = 3, + ROBOT_SECURITY_STOPPED_MODE = 4, + ROBOT_EMERGENCY_STOPPED_MODE = 5, + ROBOT_FATAL_ERROR_MODE = 6, + ROBOT_NO_POWER_MODE = 7, + ROBOT_NOT_CONNECTED_MODE = 8, + ROBOT_SHUTDOWN_MODE = 9, + ROBOT_SAFEGUARD_STOP_MODE = 10 +}; +} +typedef robot_state_type_v18::robot_state_type robotStateTypeV18; +namespace robot_state_type_v30 { +enum robot_state_type { + ROBOT_MODE_DISCONNECTED = 0, + ROBOT_MODE_CONFIRM_SAFETY = 1, + ROBOT_MODE_BOOTING = 2, + ROBOT_MODE_POWER_OFF = 3, + ROBOT_MODE_POWER_ON = 4, + ROBOT_MODE_IDLE = 5, + ROBOT_MODE_BACKDRIVE = 6, + ROBOT_MODE_RUNNING = 7, + ROBOT_MODE_UPDATING_FIRMWARE = 8 +}; +} + +typedef robot_state_type_v30::robot_state_type robotStateTypeV30; + struct version_message { uint64_t timestamp; int8_t source; @@ -88,18 +131,34 @@ struct masterboard_data { int euromapOutputBits; float euromapVoltage; float euromapCurrent; +}; +struct robot_mode_data { + uint64_t timestamp; + bool isRobotConnected; + bool isRealRobotEnabled; + bool isPowerOnRobot; + bool isEmergencyStopped; + bool isProtectiveStopped; + bool isProgramRunning; + bool isProgramPaused; + unsigned char robotMode; + unsigned char controlMode; + double targetSpeedFraction; + double speedScaling; }; class RobotState { private: version_message version_msg_; masterboard_data mb_data_; + robot_mode_data robot_mode_; std::recursive_mutex val_lock_; // Locks the variables while unpack parses data; std::condition_variable* pMsg_cond_; //Signals that new vars are available bool new_data_available_; //to avoid spurious wakes + unsigned char robot_mode_running_; double ntohd(uint64_t nf); @@ -119,6 +178,7 @@ public: char getAnalogOutputDomain1(); double getAnalogOutput0(); double getAnalogOutput1(); + std::vector getVActual(); float getMasterBoardTemperature(); float getRobotVoltage48V(); float getRobotCurrent(); @@ -130,16 +190,29 @@ public: int getEuromapOutputBits(); float getEuromapVoltage(); float getEuromapCurrent(); + + bool isRobotConnected(); + bool isRealRobotEnabled(); + bool isPowerOnRobot(); + bool isEmergencyStopped(); + bool isProtectiveStopped(); + bool isProgramRunning(); + bool isProgramPaused(); + unsigned char getRobotMode(); + bool isReady(); + + void setDisconnected(); + bool getNewDataAvailable(); void finishedReading(); - std::vector getVActual(); + void unpack(uint8_t * buf, unsigned int buf_length); void unpackRobotMessage(uint8_t * buf, unsigned int offset, uint32_t len); void unpackRobotMessageVersion(uint8_t * buf, unsigned int offset, uint32_t len); - void unpackRobotState(uint8_t * buf, unsigned int offset, uint32_t len); void unpackRobotStateMasterboard(uint8_t * buf, unsigned int offset); + void unpackRobotMode(uint8_t * buf, unsigned int offset); }; #endif /* ROBOT_STATE_H_ */ diff --git a/include/ur_modern_driver/robot_state_RT.h b/include/ur_modern_driver/robot_state_RT.h index bec0767..1f5b2af 100644 --- a/include/ur_modern_driver/robot_state_RT.h +++ b/include/ur_modern_driver/robot_state_RT.h @@ -1,12 +1,19 @@ /* * robotStateRT.h * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #ifndef ROBOT_STATE_RT_H_ diff --git a/include/ur_modern_driver/ur_communication.h b/include/ur_modern_driver/ur_communication.h index 7adb165..5173c45 100644 --- a/include/ur_modern_driver/ur_communication.h +++ b/include/ur_modern_driver/ur_communication.h @@ -1,12 +1,19 @@ /* * ur_communication.h * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #ifndef UR_COMMUNICATION_H_ diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h index de74a82..f2d9b7b 100644 --- a/include/ur_modern_driver/ur_driver.h +++ b/include/ur_modern_driver/ur_driver.h @@ -1,12 +1,19 @@ /* * ur_driver * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #ifndef UR_DRIVER_H_ @@ -39,7 +46,9 @@ private: const unsigned int REVERSE_PORT_; int incoming_sockfd_; int new_sockfd_; + bool reverse_connected_; double servoj_time_; + bool executing_traj_; public: UrRealtimeCommunication* rt_interface_; UrCommunication* sec_interface_; @@ -54,19 +63,16 @@ public: void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.); - /* void addTraj( - std::vector inp_timestamps, //DEPRECATED - std::vector > positions, - std::vector > velocities); */ - void doTraj(std::vector inp_timestamps, + + bool doTraj(std::vector inp_timestamps, std::vector > inp_positions, std::vector > inp_velocities); - void servoj(std::vector positions, int keepalive = 1, double time = 0); + void servoj(std::vector positions, int keepalive = 1); void stopTraj(); - void uploadProg(); - void openServo(); + bool uploadProg(); + bool openServo(); void closeServo(std::vector positions); std::vector interp_cubic(double t, double T, diff --git a/include/ur_modern_driver/ur_hardware_interface.h b/include/ur_modern_driver/ur_hardware_interface.h index c2de2d7..ac9f310 100644 --- a/include/ur_modern_driver/ur_hardware_interface.h +++ b/include/ur_modern_driver/ur_hardware_interface.h @@ -1,15 +1,22 @@ /* * ur_hardware_control_loop.cpp * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ -/* Based on original source from University of Colorado, Boulder. License copied below. Please offer Dave a beer as well if you meet him. */ +/* Based on original source from University of Colorado, Boulder. License copied below. */ /********************************************************************* * Software License Agreement (BSD License) @@ -58,6 +65,7 @@ #include #include #include +#include #include "do_output.h" #include "ur_driver.h" @@ -86,6 +94,8 @@ public: /// \brief write the command to the robot hardware. virtual void write(); + void setMaxVelChange(double inp); + bool canSwitch( const std::list &start_list, const std::list &stop_list) const; @@ -111,10 +121,13 @@ protected: std::vector joint_effort_; std::vector joint_position_command_; std::vector joint_velocity_command_; - std::size_t num_joints_; + std::vector prev_joint_velocity_command_; + std::size_t num_joints_; double robot_force_[3] = { 0., 0., 0. }; double robot_torque_[3] = { 0., 0., 0. }; + double max_vel_change_; + // Robot API UrDriver* robot_; diff --git a/include/ur_modern_driver/ur_realtime_communication.h b/include/ur_modern_driver/ur_realtime_communication.h index 66b5a4b..9d6b445 100644 --- a/include/ur_modern_driver/ur_realtime_communication.h +++ b/include/ur_modern_driver/ur_realtime_communication.h @@ -1,12 +1,19 @@ /* * ur_realtime_communication.h * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #ifndef UR_REALTIME_COMMUNICATION_H_ diff --git a/launch/ur10_bringup.launch b/launch/ur10_bringup.launch index ef7befd..97d5584 100644 --- a/launch/ur10_bringup.launch +++ b/launch/ur10_bringup.launch @@ -12,7 +12,7 @@ - + diff --git a/launch/ur10_ros_control.launch b/launch/ur10_ros_control.launch index f570298..ce6c1e1 100644 --- a/launch/ur10_ros_control.launch +++ b/launch/ur10_ros_control.launch @@ -25,17 +25,17 @@ - + + output="screen" args="joint_state_controller force_torque_sensor_controller vel_based_pos_traj_controller" /> + output="screen" args="load pos_based_pos_traj_controller" /> - \ No newline at end of file + diff --git a/launch/ur3_bringup.launch b/launch/ur3_bringup.launch new file mode 100644 index 0000000..1b28f72 --- /dev/null +++ b/launch/ur3_bringup.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/ur3_bringup_joint_limited.launch b/launch/ur3_bringup_joint_limited.launch new file mode 100644 index 0000000..6d40005 --- /dev/null +++ b/launch/ur3_bringup_joint_limited.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + diff --git a/launch/ur3_ros_control.launch b/launch/ur3_ros_control.launch new file mode 100644 index 0000000..1c1cd89 --- /dev/null +++ b/launch/ur3_ros_control.launch @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/ur5_ros_control.launch b/launch/ur5_ros_control.launch index b03a107..b81b017 100644 --- a/launch/ur5_ros_control.launch +++ b/launch/ur5_ros_control.launch @@ -30,11 +30,11 @@ + output="screen" args="joint_state_controller force_torque_sensor_controller vel_based_pos_traj_controller" /> + output="screen" args="load pos_based_pos_traj_controller" /> diff --git a/launch/ur_common.launch b/launch/ur_common.launch index b2ceb0a..6cd06dd 100644 --- a/launch/ur_common.launch +++ b/launch/ur_common.launch @@ -8,9 +8,10 @@ --> - - - + + + + @@ -21,9 +22,10 @@ - - - - + + + + + diff --git a/src/do_output.cpp b/src/do_output.cpp index a4b8b68..048cce1 100644 --- a/src/do_output.cpp +++ b/src/do_output.cpp @@ -1,12 +1,19 @@ /* * do_output.cpp * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #include "ur_modern_driver/do_output.h" diff --git a/src/robot_state.cpp b/src/robot_state.cpp index d56f7ca..a7eb589 100644 --- a/src/robot_state.cpp +++ b/src/robot_state.cpp @@ -1,8 +1,19 @@ /* * robot_state.cpp * - * Created on: Sep 10, 2015 - * Author: ttan + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #include "ur_modern_driver/robot_state.h" @@ -12,6 +23,8 @@ RobotState::RobotState(std::condition_variable& msg_cond) { version_msg_.minor_version = 0; new_data_available_ = false; pMsg_cond_ = &msg_cond; + RobotState::setDisconnected(); + robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING; } double RobotState::ntohd(uint64_t nf) { double x; @@ -28,7 +41,7 @@ void RobotState::unpack(uint8_t* buf, unsigned int buf_length) { memcpy(&len, &buf[offset], sizeof(len)); len = ntohl(len); if (len + offset > buf_length) { - return ; + return; } memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type)); switch (message_type) { @@ -63,7 +76,7 @@ void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset, switch (robot_message_type) { case robotMessageType::ROBOT_MESSAGE_VERSION: val_lock_.lock(); - version_msg_.timestamp = RobotState::ntohd(timestamp); + version_msg_.timestamp = timestamp; version_msg_.source = source; version_msg_.robot_message_type = robot_message_type; RobotState::unpackRobotMessageVersion(buf, offset, len); @@ -83,11 +96,18 @@ void RobotState::unpackRobotState(uint8_t * buf, unsigned int offset, uint8_t package_type; memcpy(&length, &buf[offset], sizeof(length)); length = ntohl(length); - memcpy(&package_type, &buf[offset+sizeof(length)], sizeof(package_type)); + memcpy(&package_type, &buf[offset + sizeof(length)], + sizeof(package_type)); switch (package_type) { + case packageType::ROBOT_MODE_DATA: + val_lock_.lock(); + RobotState::unpackRobotMode(buf, offset + 5); + val_lock_.unlock(); + break; + case packageType::MASTERBOARD_DATA: val_lock_.lock(); - RobotState::unpackRobotStateMasterboard(buf, offset+5); + RobotState::unpackRobotStateMasterboard(buf, offset + 5); val_lock_.unlock(); break; default: @@ -121,12 +141,78 @@ void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset, version_msg_.svn_revision = ntohl(version_msg_.svn_revision); memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset); version_msg_.build_date[len - offset] = '\0'; + if (version_msg_.major_version < 2) { + robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE; + } +} + +void RobotState::unpackRobotMode(uint8_t * buf, unsigned int offset) { + memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp)); + offset += sizeof(robot_mode_.timestamp); + uint8_t tmp; + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isRobotConnected = true; + else + robot_mode_.isRobotConnected = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isRealRobotEnabled = true; + else + robot_mode_.isRealRobotEnabled = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + //printf("PowerOnRobot: %d\n", tmp); + if (tmp > 0) + robot_mode_.isPowerOnRobot = true; + else + robot_mode_.isPowerOnRobot = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isEmergencyStopped = true; + else + robot_mode_.isEmergencyStopped = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isProtectiveStopped = true; + else + robot_mode_.isProtectiveStopped = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isProgramRunning = true; + else + robot_mode_.isProgramRunning = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isProgramPaused = true; + else + robot_mode_.isProgramPaused = false; + offset += sizeof(tmp); + memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode)); + offset += sizeof(robot_mode_.robotMode); + uint64_t temp; + if (RobotState::getVersion() > 2.) { + memcpy(&robot_mode_.controlMode, &buf[offset], + sizeof(robot_mode_.controlMode)); + offset += sizeof(robot_mode_.controlMode); + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + robot_mode_.targetSpeedFraction = RobotState::ntohd(temp); + } + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + robot_mode_.speedScaling = RobotState::ntohd(temp); } void RobotState::unpackRobotStateMasterboard(uint8_t * buf, unsigned int offset) { if (RobotState::getVersion() < 3.0) { - int16_t digital_input_bits, digital_output_bits; + int16_t digital_input_bits, digital_output_bits; memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); offset += sizeof(digital_input_bits); memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits)); @@ -263,3 +349,39 @@ double RobotState::getAnalogOutput0() { double RobotState::getAnalogOutput1() { return mb_data_.analogOutput1; } +bool RobotState::isRobotConnected() { + return robot_mode_.isRobotConnected; +} +bool RobotState::isRealRobotEnabled() { + return robot_mode_.isRealRobotEnabled; +} +bool RobotState::isPowerOnRobot() { + return robot_mode_.isPowerOnRobot; +} +bool RobotState::isEmergencyStopped() { + return robot_mode_.isEmergencyStopped; +} +bool RobotState::isProtectiveStopped() { + return robot_mode_.isProtectiveStopped; +} +bool RobotState::isProgramRunning() { + return robot_mode_.isProgramRunning; +} +bool RobotState::isProgramPaused() { + return robot_mode_.isProgramPaused; +} +unsigned char RobotState::getRobotMode() { + return robot_mode_.robotMode; +} +bool RobotState::isReady() { + if (robot_mode_.robotMode == robot_mode_running_) { + return true; + } + return false; +} + +void RobotState::setDisconnected() { + robot_mode_.isRobotConnected = false; + robot_mode_.isRealRobotEnabled = false; + robot_mode_.isPowerOnRobot = false; +} diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp index 273db0e..03921e6 100644 --- a/src/robot_state_RT.cpp +++ b/src/robot_state_RT.cpp @@ -1,12 +1,19 @@ /* * robotStateRT.cpp * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #include "ur_modern_driver/robot_state_RT.h" diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp index 3b81892..cf04cc3 100644 --- a/src/ur_communication.cpp +++ b/src/ur_communication.cpp @@ -1,12 +1,19 @@ /* * ur_communication.cpp * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #include "ur_modern_driver/ur_communication.h" @@ -37,10 +44,14 @@ UrCommunication::UrCommunication(std::condition_variable& msg_cond, flag_ = 1; setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); + setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, + sizeof(int)); setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, + sizeof(int)); setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); @@ -62,7 +73,7 @@ bool UrCommunication::start() { } print_debug("Acquire firmware version: Got connection"); bytes_read = read(pri_sockfd_, buf, 512); - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, + setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, sizeof(int)); robot_state_->unpack(buf, bytes_read); //wait for some traffic so the UR socket doesn't die in version 3.1. @@ -117,11 +128,12 @@ void UrCommunication::run() { select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout); bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes if (bytes_read > 0) { - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, sizeof(int)); robot_state_->unpack(buf, bytes_read); } else { connected_ = false; + robot_state_->setDisconnected(); close(sec_sockfd_); } } @@ -135,13 +147,14 @@ void UrCommunication::run() { flag_ = 1; setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, + sizeof(int)); setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); while (keepalive_ && !connected_) { std::this_thread::sleep_for(std::chrono::seconds(10)); fd_set writefds; - keepalive_ = true; connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_, sizeof(sec_serv_addr_)); @@ -155,6 +168,7 @@ void UrCommunication::run() { print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds..."); } else { connected_ = true; + print_info("Secondary port: Reconnected"); } } } diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index c141ec3..70503e9 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -1,27 +1,37 @@ /* * ur_driver.cpp * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #include "ur_modern_driver/ur_driver.h" UrDriver::UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host, - unsigned int reverse_port, double servoj_time, unsigned int safety_count_max, - double max_time_step, double min_payload, double max_payload) : + unsigned int reverse_port, double servoj_time, + unsigned int safety_count_max, double max_time_step, double min_payload, + double max_payload) : REVERSE_PORT_(reverse_port), maximum_time_step_(max_time_step), minimum_payload_( - min_payload), maximum_payload_(max_payload), servoj_time_(servoj_time) { + min_payload), maximum_payload_(max_payload), servoj_time_( + servoj_time) { char buffer[256]; struct sockaddr_in serv_addr; int n, flag; - //char *ip_addr; + reverse_connected_ = false; + executing_traj_ = false; rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, safety_count_max); new_sockfd_ = -1; @@ -63,76 +73,24 @@ std::vector UrDriver::interp_cubic(double t, double T, } return positions; } - /* -void UrDriver::addTraj(std::vector inp_timestamps, + +bool UrDriver::doTraj(std::vector inp_timestamps, std::vector > inp_positions, std::vector > inp_velocities) { - // DEPRECATED - printf("!! addTraj is deprecated !!\n"); - std::vector timestamps; - std::vector > positions; - std::string command_string = "def traj():\n"; - - for (unsigned int i = 1; i < inp_timestamps.size(); i++) { - timestamps.push_back(inp_timestamps[i - 1]); - double dt = inp_timestamps[i] - inp_timestamps[i - 1]; - unsigned int steps = (unsigned int) ceil(dt / maximum_time_step_); - double step_size = dt / steps; - for (unsigned int j = 1; j < steps; j++) { - timestamps.push_back(inp_timestamps[i - 1] + step_size * j); - } - } - // //make sure we come to a smooth stop - // while (timestamps.back() < inp_timestamps.back()) { - // timestamps.push_back(timestamps.back() + 0.008); - // } - // timestamps.pop_back(); - - unsigned int j = 0; - for (unsigned int i = 0; i < timestamps.size(); i++) { - while (inp_timestamps[j] <= timestamps[i]) { - j += 1; - } - positions.push_back( - UrDriver::interp_cubic(timestamps[i] - inp_timestamps[j - 1], - inp_timestamps[j] - inp_timestamps[j - 1], - inp_positions[j - 1], inp_positions[j], - inp_velocities[j - 1], inp_velocities[j])); - } - - timestamps.push_back(inp_timestamps[inp_timestamps.size() - 1]); - positions.push_back(inp_positions[inp_positions.size() - 1]); - /// This is actually faster than using a stringstream :-o - for (unsigned int i = 1; i < timestamps.size(); i++) { - char buf[128]; - sprintf(buf, - "\tservoj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], t=%1.5f)\n", - positions[i][0], positions[i][1], positions[i][2], - positions[i][3], positions[i][4], positions[i][5], - timestamps[i] - timestamps[i - 1]); - command_string += buf; - } - command_string += "end\n"; - //printf("%s", command_string.c_str()); - rt_interface_->addCommandToQueue(command_string); - -} -*/ -void UrDriver::doTraj(std::vector inp_timestamps, - std::vector > inp_positions, - std::vector > inp_velocities) { - std::chrono::high_resolution_clock::time_point t0, t; std::vector positions; unsigned int j; - UrDriver::uploadProg(); - + if (!UrDriver::uploadProg()) { + return false; + } + executing_traj_ = true; t0 = std::chrono::high_resolution_clock::now(); t = t0; j = 0; - while (inp_timestamps[inp_timestamps.size() - 1] - >= std::chrono::duration_cast>(t - t0).count()) { + while ((inp_timestamps[inp_timestamps.size() - 1] + >= std::chrono::duration_cast>(t - t0).count()) + and executing_traj_) { while (inp_timestamps[j] <= std::chrono::duration_cast>( t - t0).count() && j < inp_timestamps.size() - 1) { @@ -143,25 +101,29 @@ void UrDriver::doTraj(std::vector inp_timestamps, t - t0).count() - inp_timestamps[j - 1], inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1], inp_positions[j], inp_velocities[j - 1], inp_velocities[j]); - UrDriver::servoj(positions); // oversample with 4 * sample_time - std::this_thread::sleep_for(std::chrono::milliseconds((int) ((servoj_time_*1000)/4.))); + std::this_thread::sleep_for( + std::chrono::milliseconds((int) ((servoj_time_ * 1000) / 4.))); t = std::chrono::high_resolution_clock::now(); } + executing_traj_ = false; //Signal robot to stop driverProg() UrDriver::closeServo(positions); + return true; } -void UrDriver::servoj(std::vector positions, - int keepalive, double time) { +void UrDriver::servoj(std::vector positions, int keepalive) { + if (!reverse_connected_) { + print_error( + "UrDriver::servoj called without a reverse connection present. Keepalive: " + + std::to_string(keepalive)); + return; + } unsigned int bytes_written; int tmp; - unsigned char buf[32]; - if (time < 0.016) { - time = servoj_time_; - } + unsigned char buf[28]; for (int i = 0; i < 6; i++) { tmp = htonl((int) (positions[i] * MULT_JOINTSTATE_)); buf[i * 4] = tmp & 0xff; @@ -169,24 +131,20 @@ void UrDriver::servoj(std::vector positions, buf[i * 4 + 2] = (tmp >> 16) & 0xff; buf[i * 4 + 3] = (tmp >> 24) & 0xff; } - tmp = htonl((int) (time * MULT_TIME_)); + tmp = htonl((int) keepalive); buf[6 * 4] = tmp & 0xff; buf[6 * 4 + 1] = (tmp >> 8) & 0xff; buf[6 * 4 + 2] = (tmp >> 16) & 0xff; buf[6 * 4 + 3] = (tmp >> 24) & 0xff; - tmp = htonl((int) keepalive); - buf[7 * 4] = tmp & 0xff; - buf[7 * 4 + 1] = (tmp >> 8) & 0xff; - buf[7 * 4 + 2] = (tmp >> 16) & 0xff; - buf[7 * 4 + 3] = (tmp >> 24) & 0xff; - bytes_written = write(new_sockfd_, buf, 32); + bytes_written = write(new_sockfd_, buf, 28); } void UrDriver::stopTraj() { + executing_traj_ = false; rt_interface_->addCommandToQueue("stopj(10)\n"); } -void UrDriver::uploadProg() { +bool UrDriver::uploadProg() { std::string cmd_str; char buf[128]; cmd_str = "def driverProg():\n"; @@ -194,20 +152,14 @@ void UrDriver::uploadProg() { sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_); cmd_str += buf; - sprintf(buf, "\tMULT_time = %i\n", MULT_TIME_); - cmd_str += buf; - cmd_str += "\tSERVO_IDLE = 0\n"; cmd_str += "\tSERVO_RUNNING = 1\n"; cmd_str += "\tcmd_servo_state = SERVO_IDLE\n"; - cmd_str += "\tcmd_servo_id = 0 # 0 = idle, -1 = stop\n"; cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n"; - cmd_str += "\tcmd_servo_dt = 0.0\n"; - cmd_str += "\tdef set_servo_setpoint(q, dt):\n"; + cmd_str += "\tdef set_servo_setpoint(q):\n"; cmd_str += "\t\tenter_critical\n"; cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n"; cmd_str += "\t\tcmd_servo_q = q\n"; - cmd_str += "\t\tcmd_servo_dt = dt\n"; cmd_str += "\t\texit_critical\n"; cmd_str += "\tend\n"; cmd_str += "\tthread servoThread():\n"; @@ -215,7 +167,6 @@ void UrDriver::uploadProg() { cmd_str += "\t\twhile True:\n"; cmd_str += "\t\t\tenter_critical\n"; cmd_str += "\t\t\tq = cmd_servo_q\n"; - cmd_str += "\t\t\tdt = cmd_servo_dt\n"; cmd_str += "\t\t\tdo_brake = False\n"; cmd_str += "\t\t\tif (state == SERVO_RUNNING) and "; cmd_str += "(cmd_servo_state == SERVO_IDLE):\n"; @@ -228,7 +179,13 @@ void UrDriver::uploadProg() { cmd_str += "\t\t\t\tstopj(1.0)\n"; cmd_str += "\t\t\t\tsync()\n"; cmd_str += "\t\t\telif state == SERVO_RUNNING:\n"; - cmd_str += "\t\t\t\tservoj(q, t=dt)\n"; + + if (sec_interface_->robot_state_->getVersion() >= 3.1) + sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=0.03)\n", servoj_time_); + else + sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_); + cmd_str += buf; + cmd_str += "\t\t\telse:\n"; cmd_str += "\t\t\t\tsync()\n"; cmd_str += "\t\t\tend\n"; @@ -242,7 +199,7 @@ void UrDriver::uploadProg() { cmd_str += "\tthread_servo = run servoThread()\n"; cmd_str += "\tkeepalive = 1\n"; cmd_str += "\twhile keepalive > 0:\n"; - cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1+1)\n"; + cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n"; cmd_str += "\t\tif params_mult[0] > 0:\n"; cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, "; cmd_str += "params_mult[2] / MULT_jointstate, "; @@ -250,9 +207,8 @@ void UrDriver::uploadProg() { cmd_str += "params_mult[4] / MULT_jointstate, "; cmd_str += "params_mult[5] / MULT_jointstate, "; cmd_str += "params_mult[6] / MULT_jointstate]\n"; - cmd_str += "\t\t\tt = params_mult[7] / MULT_time\n"; - cmd_str += "\t\t\tkeepalive = params_mult[8]\n"; - cmd_str += "\t\t\tset_servo_setpoint(q, t)\n"; + cmd_str += "\t\t\tkeepalive = params_mult[7]\n"; + cmd_str += "\t\t\tset_servo_setpoint(q)\n"; cmd_str += "\t\tend\n"; cmd_str += "\tend\n"; cmd_str += "\tsleep(.1)\n"; @@ -260,10 +216,10 @@ void UrDriver::uploadProg() { cmd_str += "end\n"; rt_interface_->addCommandToQueue(cmd_str); - UrDriver::openServo(); + return UrDriver::openServo(); } -void UrDriver::openServo() { +bool UrDriver::openServo() { struct sockaddr_in cli_addr; socklen_t clilen; clilen = sizeof(cli_addr); @@ -271,13 +227,18 @@ void UrDriver::openServo() { &clilen); if (new_sockfd_ < 0) { print_fatal("ERROR on accepting reverse communication"); + return false; } + reverse_connected_ = true; + return true; } void UrDriver::closeServo(std::vector positions) { if (positions.size() != 6) UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0); else UrDriver::servoj(positions, 0); + + reverse_connected_ = false; close(new_sockfd_); } @@ -288,15 +249,18 @@ bool UrDriver::start() { sec_interface_->robot_state_->getVersion()); if (!rt_interface_->start()) return false; - ip_addr_ = rt_interface_->getLocalIp(); //inet_ntoa(serv_addr.sin_addr); - char buf[256]; - sprintf(buf, "Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_); - print_debug(buf); + ip_addr_ = rt_interface_->getLocalIp(); + print_debug( + "Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_) + + "\n"); return true; } void UrDriver::halt() { + if (executing_traj_) { + UrDriver::stopTraj(); + } sec_interface_->halt(); rt_interface_->halt(); close(incoming_sockfd_); @@ -366,9 +330,9 @@ void UrDriver::setMaxPayload(double m) { maximum_payload_ = m; } void UrDriver::setServojTime(double t) { - if (t > 0.016) { + if (t > 0.008) { servoj_time_ = t; } else { - servoj_time_ = 0.016; + servoj_time_ = 0.008; } } diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp index 60dfadc..ea66395 100644 --- a/src/ur_hardware_interface.cpp +++ b/src/ur_hardware_interface.cpp @@ -1,15 +1,22 @@ /* * ur_hardware_control_loop.cpp * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ -/* Based on original source from University of Colorado, Boulder. License copied below. Please offer Dave a beer as well if you meet him. */ +/* Based on original source from University of Colorado, Boulder. License copied below. */ /********************************************************************* * Software License Agreement (BSD License) @@ -57,6 +64,8 @@ UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) : // Initialize shared memory and interfaces here init(); // this implementation loads from rosparam + max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2 + ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface."); } @@ -79,6 +88,7 @@ void UrHardwareInterface::init() { joint_effort_.resize(num_joints_); joint_position_command_.resize(num_joints_); joint_velocity_command_.resize(num_joints_); + prev_joint_velocity_command_.resize(num_joints_); // Initialize controller for (std::size_t i = 0; i < num_joints_; ++i) { @@ -102,6 +112,7 @@ void UrHardwareInterface::init() { hardware_interface::JointHandle( joint_state_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i])); + prev_joint_velocity_command_[i] = 0.; } // Create force torque interface @@ -135,11 +146,26 @@ void UrHardwareInterface::read() { } +void UrHardwareInterface::setMaxVelChange(double inp) { + max_vel_change_ = inp; +} + void UrHardwareInterface::write() { 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); + std::vector cmd; + //do some rate limiting + cmd.resize(joint_velocity_command_.size()); + for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) { + cmd[i] = joint_velocity_command_[i]; + if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) { + cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_; + } else if (cmd[i] + < prev_joint_velocity_command_[i] - max_vel_change_) { + cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_; + } + prev_joint_velocity_command_[i] = cmd[i]; + } + robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_*125); } else if (position_interface_running_) { robot_->servoj(joint_position_command_); } diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index 9f12b70..f841455 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -1,12 +1,19 @@ /* * ur_realtime_communication.cpp * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #include "ur_modern_driver/ur_realtime_communication.h" @@ -29,6 +36,7 @@ UrRealtimeCommunication::UrRealtimeCommunication( serv_addr_.sin_port = htons(30003); flag_ = 1; setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); + setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, sizeof(int)); setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); fcntl(sockfd_, F_SETFL, O_NONBLOCK); connected_ = false; @@ -81,7 +89,10 @@ void UrRealtimeCommunication::addCommandToQueue(std::string inp) { if (inp.back() != '\n') { inp.append("\n"); } - bytes_written = write(sockfd_, inp.c_str(), inp.length()); + if (connected_) + bytes_written = write(sockfd_, inp.c_str(), inp.length()); + else + print_error("Could not send command \"" +inp + "\". The robot is not connected! Command is discarded" ); } void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2, @@ -114,7 +125,7 @@ void UrRealtimeCommunication::run() { select(sockfd_ + 1, &readfds, NULL, NULL, &timeout); bytes_read = read(sockfd_, buf, 2048); if (bytes_read > 0) { - setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, + setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, sizeof(int)); robot_state_->unpack(buf); if (safety_count_ == safety_count_max_) { @@ -136,13 +147,15 @@ void UrRealtimeCommunication::run() { flag_ = 1; setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); + setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, + sizeof(int)); + setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); fcntl(sockfd_, F_SETFL, O_NONBLOCK); while (keepalive_ && !connected_) { std::this_thread::sleep_for(std::chrono::seconds(10)); fd_set writefds; - keepalive_ = true; connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_)); @@ -155,6 +168,7 @@ void UrRealtimeCommunication::run() { print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds..."); } else { connected_ = true; + print_info("Realtime port: Reconnected"); } } } diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index c782bde..b9f15ca 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -1,12 +1,19 @@ /* * ur_driver.cpp * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #include "ur_modern_driver/ur_driver.h" @@ -19,6 +26,7 @@ #include #include #include +#include #include #include "ros/ros.h" @@ -26,7 +34,8 @@ #include "sensor_msgs/JointState.h" #include "geometry_msgs/WrenchStamped.h" #include "control_msgs/FollowJointTrajectoryAction.h" -#include "actionlib/server/simple_action_server.h" +#include "actionlib/server/action_server.h" +#include "actionlib/server/server_goal_handle.h" #include "trajectory_msgs/JointTrajectoryPoint.h" #include "ur_msgs/SetIO.h" #include "ur_msgs/SetPayload.h" @@ -46,8 +55,9 @@ protected: std::condition_variable rt_msg_cond_; std::condition_variable msg_cond_; ros::NodeHandle nh_; - actionlib::SimpleActionServer as_; - actionlib::SimpleActionServer::Goal goal_; + actionlib::ActionServer as_; + actionlib::ServerGoalHandle goal_handle_; + bool has_goal_; control_msgs::FollowJointTrajectoryFeedback feedback_; control_msgs::FollowJointTrajectoryResult result_; ros::Subscriber speed_sub_; @@ -65,10 +75,12 @@ protected: boost::shared_ptr controller_manager_; public: - RosWrapper(std::string host) : - as_(nh_, "follow_joint_trajectory", false), robot_(rt_msg_cond_, - msg_cond_, host), io_flag_delay_(0.05), joint_offsets_(6, - 0.0) { + RosWrapper(std::string host, int reverse_port) : + as_(nh_, "follow_joint_trajectory", + boost::bind(&RosWrapper::goalCB, this, _1), + boost::bind(&RosWrapper::cancelCB, this, _1), false), robot_( + rt_msg_cond_, msg_cond_, host, reverse_port), io_flag_delay_(0.05), joint_offsets_( + 6, 0.0) { std::string joint_prefix = ""; std::vector joint_names; @@ -95,6 +107,14 @@ public: controller_manager_.reset( new controller_manager::ControllerManager( hardware_interface_.get(), nh_)); + double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2 + if (ros::param::get("~max_acceleration", max_vel_change)) { + max_vel_change = max_vel_change / 125; + } + sprintf(buf, "Max acceleration set to: %f [rad/sec²]", + max_vel_change * 125); + print_debug(buf); + hardware_interface_->setMaxVelChange(max_vel_change); } //Using a very high value in order to not limit execution of trajectories being sent from MoveIt! max_velocity_ = 10.; @@ -122,7 +142,7 @@ public: min_payload, max_payload); print_debug(buf); - double servoj_time = 0.016; + double servoj_time = 0.008; if (ros::param::get("~servoj_time", servoj_time)) { sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time); print_debug(buf); @@ -136,12 +156,8 @@ public: print_debug( "The control thread for this driver has been started"); } else { - //register the goal and feedback callbacks - as_.registerGoalCallback( - boost::bind(&RosWrapper::goalCB, this)); - as_.registerPreemptCallback( - boost::bind(&RosWrapper::preemptCB, this)); - + //start actionserver + has_goal_ = false; as_.start(); //subscribe to the data topic of interest @@ -170,75 +186,167 @@ public: } private: - void goalCB() { - print_info("on_goal"); + void trajThread(std::vector timestamps, + std::vector > positions, + std::vector > velocities) { - actionlib::SimpleActionServer::GoalConstPtr goal = - as_.acceptNewGoal(); - goal_ = *goal; //make a copy that we can modify + robot_.doTraj(timestamps, positions, velocities); + if (has_goal_) { + result_.error_code = result_.SUCCESSFUL; + goal_handle_.setSucceeded(result_); + has_goal_ = false; + } + } + void goalCB( + actionlib::ServerGoalHandle< + control_msgs::FollowJointTrajectoryAction> gh) { + std::string buf; + print_info("on_goal"); + if (!robot_.sec_interface_->robot_state_->isReady()) { + result_.error_code = -100; //nothing is defined for this...? + + if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) { + result_.error_string = + "Cannot accept new trajectories: Robot arm is not powered on"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) { + result_.error_string = + "Cannot accept new trajectories: Robot is not enabled"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + result_.error_string = + "Cannot accept new trajectories. (Debug: Robot mode is " + + std::to_string( + robot_.sec_interface_->robot_state_->getRobotMode()) + + ")"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) { + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = + "Cannot accept new trajectories: Robot is emergency stopped"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) { + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = + "Cannot accept new trajectories: Robot is protective stopped"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + + actionlib::ActionServer::Goal goal = + *gh.getGoal(); //make a copy that we can modify + if (has_goal_) { + print_warning( + "Received new goal while still executing previous trajectory. Canceling previous trajectory"); + has_goal_ = false; + robot_.stopTraj(); + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = "Received another trajectory"; + goal_handle_.setAborted(result_, result_.error_string); + std::this_thread::sleep_for(std::chrono::milliseconds(250)); + } + goal_handle_ = gh; if (!validateJointNames()) { std::string outp_joint_names = ""; - for (unsigned int i = 0; i < goal_.trajectory.joint_names.size(); + for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) { - outp_joint_names += goal_.trajectory.joint_names[i] + " "; + outp_joint_names += goal.trajectory.joint_names[i] + " "; } result_.error_code = result_.INVALID_JOINTS; result_.error_string = "Received a goal with incorrect joint names: " + outp_joint_names; - as_.setAborted(result_, result_.error_string); + gh.setRejected(result_, result_.error_string); print_error(result_.error_string); + return; } - if (!has_positions()) { result_.error_code = result_.INVALID_GOAL; result_.error_string = "Received a goal without positions"; - as_.setAborted(result_, result_.error_string); + gh.setRejected(result_, result_.error_string); print_error(result_.error_string); + return; } if (!has_velocities()) { result_.error_code = result_.INVALID_GOAL; result_.error_string = "Received a goal without velocities"; - as_.setAborted(result_, result_.error_string); + gh.setRejected(result_, result_.error_string); print_error(result_.error_string); + return; } if (!traj_is_finite()) { - result_.error_string = "Received a goal with infinites or NaNs"; + result_.error_string = "Received a goal with infinities or NaNs"; result_.error_code = result_.INVALID_GOAL; - as_.setAborted(result_, result_.error_string); + gh.setRejected(result_, result_.error_string); print_error(result_.error_string); + return; } if (!has_limited_velocities()) { result_.error_code = result_.INVALID_GOAL; result_.error_string = - "Received a goal with velocities that are higher than %f", max_velocity_; - as_.setAborted(result_, result_.error_string); + "Received a goal with velocities that are higher than " + + std::to_string(max_velocity_); + gh.setRejected(result_, result_.error_string); print_error(result_.error_string); + return; } - reorder_traj_joints(goal_.trajectory); + reorder_traj_joints(goal.trajectory); + std::vector timestamps; std::vector > positions, velocities; - for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) { + if (goal.trajectory.points[0].time_from_start.toSec() != 0.) { + print_warning( + "Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory"); + timestamps.push_back(0.0); + positions.push_back( + robot_.rt_interface_->robot_state_->getQActual()); + velocities.push_back( + robot_.rt_interface_->robot_state_->getQdActual()); + } + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { timestamps.push_back( - goal_.trajectory.points[i].time_from_start.toSec()); - positions.push_back(goal_.trajectory.points[i].positions); - velocities.push_back(goal_.trajectory.points[i].velocities); + goal.trajectory.points[i].time_from_start.toSec()); + positions.push_back(goal.trajectory.points[i].positions); + velocities.push_back(goal.trajectory.points[i].velocities); } - robot_.doTraj(timestamps, positions, velocities); - result_.error_code = result_.SUCCESSFUL; - as_.setSucceeded(result_); + + goal_handle_.setAccepted(); + has_goal_ = true; + std::thread(&RosWrapper::trajThread, this, timestamps, positions, + velocities).detach(); } - void preemptCB() { - print_info("on_cancel"); + void cancelCB( + actionlib::ServerGoalHandle< + control_msgs::FollowJointTrajectoryAction> gh) { // set the action state to preempted - robot_.stopTraj(); - as_.setPreempted(); + print_info("on_cancel"); + if (has_goal_) { + if (gh == goal_handle_) { + robot_.stopTraj(); + has_goal_ = false; + } + } + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = "Goal cancelled by client"; + gh.setCanceled(result_); } bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) { @@ -274,16 +382,18 @@ private: bool validateJointNames() { std::vector actual_joint_names = robot_.getJointNames(); - if (goal_.trajectory.joint_names.size() != actual_joint_names.size()) + actionlib::ActionServer::Goal goal = + *goal_handle_.getGoal(); + if (goal.trajectory.joint_names.size() != actual_joint_names.size()) return false; - for (unsigned int i = 0; i < goal_.trajectory.joint_names.size(); i++) { + for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) { unsigned int j; for (j = 0; j < actual_joint_names.size(); j++) { - if (goal_.trajectory.joint_names[i] == actual_joint_names[j]) + if (goal.trajectory.joint_names[i] == actual_joint_names[j]) break; } - if (goal_.trajectory.joint_names[i] == actual_joint_names[j]) { + if (goal.trajectory.joint_names[i] == actual_joint_names[j]) { actual_joint_names.erase(actual_joint_names.begin() + j); } else { return false; @@ -324,30 +434,36 @@ private: } bool has_velocities() { - for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) { - if (goal_.trajectory.points[i].positions.size() - != goal_.trajectory.points[i].velocities.size()) + actionlib::ActionServer::Goal goal = + *goal_handle_.getGoal(); + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { + if (goal.trajectory.points[i].positions.size() + != goal.trajectory.points[i].velocities.size()) return false; } return true; } bool has_positions() { - if (goal_.trajectory.points.size() == 0) + actionlib::ActionServer::Goal goal = + *goal_handle_.getGoal(); + if (goal.trajectory.points.size() == 0) return false; - for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) { - if (goal_.trajectory.points[i].positions.size() - != goal_.trajectory.joint_names.size()) + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { + if (goal.trajectory.points[i].positions.size() + != goal.trajectory.joint_names.size()) return false; } return true; } bool has_limited_velocities() { - for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) { + actionlib::ActionServer::Goal goal = + *goal_handle_.getGoal(); + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { for (unsigned int j = 0; - j < goal_.trajectory.points[i].velocities.size(); j++) { - if (fabs(goal_.trajectory.points[i].velocities[j]) + j < goal.trajectory.points[i].velocities.size(); j++) { + if (fabs(goal.trajectory.points[i].velocities[j]) > max_velocity_) return false; } @@ -356,12 +472,14 @@ private: } bool traj_is_finite() { - for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) { + actionlib::ActionServer::Goal goal = + *goal_handle_.getGoal(); + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { for (unsigned int j = 0; - j < goal_.trajectory.points[i].velocities.size(); j++) { - if (!std::isfinite(goal_.trajectory.points[i].positions[j])) + j < goal.trajectory.points[i].velocities.size(); j++) { + if (!std::isfinite(goal.trajectory.points[i].positions[j])) return false; - if (!std::isfinite(goal_.trajectory.points[i].velocities[j])) + if (!std::isfinite(goal.trajectory.points[i].velocities[j])) return false; } } @@ -459,6 +577,7 @@ private: } void publishMbMsg() { + bool warned = false; ros::Publisher io_pub = nh_.advertise( "ur_driver/io_states", 1); @@ -500,6 +619,27 @@ private: io_msg.analog_out_states.push_back(ana); io_pub.publish(io_msg); + if (robot_.sec_interface_->robot_state_->isEmergencyStopped() + or robot_.sec_interface_->robot_state_->isProtectiveStopped()) { + if (robot_.sec_interface_->robot_state_->isEmergencyStopped() + and !warned) { + print_error("Emergency stop pressed!"); + } else if (robot_.sec_interface_->robot_state_->isProtectiveStopped() + and !warned) { + print_error("Robot is protective stopped!"); + } + if (has_goal_) { + print_error("Aborting trajectory"); + robot_.stopTraj(); + result_.error_code = result_.SUCCESSFUL; + result_.error_string = "Robot was halted"; + goal_handle_.setAborted(result_, result_.error_string); + has_goal_ = false; + } + warned = true; + } else + warned = false; + robot_.sec_interface_->robot_state_->finishedReading(); } @@ -510,6 +650,7 @@ private: int main(int argc, char **argv) { bool use_sim_time = false; std::string host; + int reverse_port = 50001; ros::init(argc, argv, "ur_driver"); ros::NodeHandle nh; @@ -528,8 +669,15 @@ int main(int argc, char **argv) { } } + if ((ros::param::get("~reverse_port", reverse_port))) { + if((reverse_port <= 0) or (reverse_port >= 65535)) { + print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001"); + reverse_port = 50001; + } + } else + reverse_port = 50001; - RosWrapper interface(host); + RosWrapper interface(host, reverse_port); ros::AsyncSpinner spinner(3); spinner.start();