Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 10:00:48 +02:00
Code Releases Activity

Merge remote-tracking branch 'thomas_timm/master' into ur3_support

Browse Source
This commit is contained in:
Robot Printer
2015-10-28 10:06:31 +01:00
parent 58361900b8 c3f3da922a
commit fd2d13ef84
22 changed files with 996 additions and 307 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
CMakeLists.txt
View File

@@ -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

181
LICENSE
View File

@@ -1,7 +1,180 @@
"THE BEER-WARE LICENSE" (Revision 42):
Copyright 2015 Thomas Timm Andersen
<thomas.timm.dk@gmail.com> 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

10
README.md
View File

@@ -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

55
config/ur10_controllers.yaml
View File

@@ -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

18
config/ur5_controllers.yaml
View File

@@ -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

19
include/ur_modern_driver/do_output.h
View File

@@ -1,12 +1,19 @@
/*
* do_output.h
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> 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_

81
include/ur_modern_driver/robot_state.h
View File

@@ -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<double> 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<double> 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_ */

19
include/ur_modern_driver/robot_state_RT.h
View File

@@ -1,12 +1,19 @@
/*
* robotStateRT.h
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> 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_

19
include/ur_modern_driver/ur_communication.h
View File

@@ -1,12 +1,19 @@
/*
* ur_communication.h
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> 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_

29
include/ur_modern_driver/ur_driver.h
View File

@@ -1,12 +1,19 @@
/*
* ur_driver
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> 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_;
@@ -58,15 +67,15 @@ public:
std::vector<double> inp_timestamps, //DEPRECATED
std::vector<std::vector<double> > positions,
std::vector<std::vector<double> > velocities); */
void doTraj(std::vector<double> inp_timestamps,
bool doTraj(std::vector<double> inp_timestamps,
std::vector<std::vector<double> > inp_positions,
std::vector<std::vector<double> > inp_velocities);
void servoj(std::vector<double> positions, int keepalive = 1, double time = 0);
void servoj(std::vector<double> positions, int keepalive = 1);
void stopTraj();
void uploadProg();
void openServo();
bool uploadProg();
bool openServo();
void closeServo(std::vector<double> positions);
std::vector<double> interp_cubic(double t, double T,

29
include/ur_modern_driver/ur_hardware_interface.h
View File

@@ -1,15 +1,22 @@
/*
* ur_hardware_control_loop.cpp
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> 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 <controller_manager/controller_manager.h>
#include <boost/scoped_ptr.hpp>
#include <ros/ros.h>
#include <math.h>
#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<hardware_interface::ControllerInfo> &start_list,
const std::list<hardware_interface::ControllerInfo> &stop_list) const;
@@ -111,10 +121,13 @@ protected:
std::vector<double> joint_effort_;
std::vector<double> joint_position_command_;
std::vector<double> joint_velocity_command_;
std::size_t num_joints_;
std::vector<double> 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_;

19
include/ur_modern_driver/ur_realtime_communication.h
View File

@@ -1,12 +1,19 @@
/*
* ur_realtime_communication.h
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> 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_

8
launch/ur10_ros_control.launch
View File

@@ -25,17 +25,17 @@
</node>
<!-- Load controller settings -->
<rosparam file="$(find ur_modern_driver)/config/ur5_controllers.yaml" command="load"/>
<rosparam file="$(find ur_modern_driver)/config/ur10_controllers.yaml" command="load"/>
<!-- spawn controller manager -->
<node name="ros_control_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" args="joint_state_controller force_torque_sensor_controller velocity_based_position_trajectory_controller" />
output="screen" args="joint_state_controller force_torque_sensor_controller vel_based_pos_traj_controller" />
<!-- load other controller -->
<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
output="screen" args="load position_based_position_trajectory_controller" />
output="screen" args="load pos_based_pos_traj_controller" />
<!-- Convert joint states to /tf tranforms -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
</launch>
</launch>

4
launch/ur5_ros_control.launch
View File

@@ -30,11 +30,11 @@
<!-- spawn controller manager -->
<node name="ros_control_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" args="joint_state_controller force_torque_sensor_controller velocity_based_position_trajectory_controller" />
output="screen" args="joint_state_controller force_torque_sensor_controller vel_based_pos_traj_controller" />
<!-- load other controller -->
<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
output="screen" args="load position_based_position_trajectory_controller" />
output="screen" args="load pos_based_pos_traj_controller" />
<!-- Convert joint states to /tf tranforms -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

19
src/do_output.cpp
View File

@@ -1,12 +1,19 @@
/*
* do_output.cpp
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> 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"

136
src/robot_state.cpp
View File

@@ -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;
}

19
src/robot_state_RT.cpp
View File

@@ -1,12 +1,19 @@
/*
* robotStateRT.cpp
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> 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"

31
src/ur_communication.cpp
View File

@@ -1,12 +1,19 @@
/*
* ur_communication.cpp
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> 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,6 +147,8 @@ 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);
@@ -155,6 +169,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");
}
}
}

232
src/ur_driver.cpp
View File

@@ -1,27 +1,37 @@
/*
* ur_driver.cpp
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> 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,78 @@ std::vector<double> UrDriver::interp_cubic(double t, double T,
}
return positions;
}
/*
void UrDriver::addTraj(std::vector<double> inp_timestamps,
/*
void UrDriver::addTraj(std::vector<double> inp_timestamps,
std::vector<std::vector<double> > inp_positions,
std::vector<std::vector<double> > inp_velocities) {
// DEPRECATED
printf("!! addTraj is deprecated !!\n");
std::vector<double> timestamps;
std::vector<std::vector<double> > 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);
}
*/
bool UrDriver::doTraj(std::vector<double> inp_timestamps,
std::vector<std::vector<double> > inp_positions,
std::vector<std::vector<double> > inp_velocities) {
// DEPRECATED
printf("!! addTraj is deprecated !!\n");
std::vector<double> timestamps;
std::vector<std::vector<double> > 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<double> inp_timestamps,
std::vector<std::vector<double> > inp_positions,
std::vector<std::vector<double> > inp_velocities) {
std::chrono::high_resolution_clock::time_point t0, t;
std::vector<double> 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<std::chrono::duration<double>>(t - t0).count()) {
while ((inp_timestamps[inp_timestamps.size() - 1]
>= std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count())
and executing_traj_) {
while (inp_timestamps[j]
<= std::chrono::duration_cast<std::chrono::duration<double>>(
t - t0).count() && j < inp_timestamps.size() - 1) {
@@ -143,25 +155,29 @@ void UrDriver::doTraj(std::vector<double> 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<double> positions,
int keepalive, double time) {
void UrDriver::servoj(std::vector<double> 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 +185,20 @@ void UrDriver::servoj(std::vector<double> 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 +206,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 +221,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 +233,10 @@ 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";
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 +250,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,20 +258,20 @@ 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 += "\tstopj(10)\n";
cmd_str += "\tsleep(.1)\n";
cmd_str += "\tsocket_close()\n";
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 +279,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<double> 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 +301,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 +382,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;
}
}

50
src/ur_hardware_interface.cpp
View File

@@ -1,15 +1,22 @@
/*
* ur_hardware_control_loop.cpp
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> 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,30 @@ 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<double> 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_;
}
if (cmd[i] > M_PI/2.)
cmd[i] = M_PI/2.;
else if (cmd[i] < -M_PI/2.)
cmd[i] = -M_PI/2.;
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_);
}

26
src/ur_realtime_communication.cpp
View File

@@ -1,12 +1,19 @@
/*
* ur_realtime_communication.cpp
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> 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;
@@ -114,7 +122,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,6 +144,9 @@ 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);
@@ -155,6 +166,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");
}
}
}

269
src/ur_ros_wrapper.cpp
View File

@@ -1,12 +1,19 @@
/*
* ur_driver.cpp
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> 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 <thread>
#include <algorithm>
#include <cmath>
#include <chrono>
#include <time.h>
#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<control_msgs::FollowJointTrajectoryAction> as_;
actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal_;
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> as_;
actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> goal_handle_;
bool has_goal_;
control_msgs::FollowJointTrajectoryFeedback feedback_;
control_msgs::FollowJointTrajectoryResult result_;
ros::Subscriber speed_sub_;
@@ -66,9 +76,11 @@ protected:
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) {
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), io_flag_delay_(0.05), joint_offsets_(
6, 0.0) {
std::string joint_prefix = "";
std::vector<std::string> 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.08;
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,68 +186,162 @@ public:
}
private:
void goalCB() {
print_info("on_goal");
void trajThread(std::vector<double> timestamps,
std::vector<std::vector<double> > positions,
std::vector<std::vector<double> > velocities) {
actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>::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<control_msgs::FollowJointTrajectoryAction>::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";
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<double> timestamps;
std::vector<std::vector<double> > 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) {
@@ -267,16 +377,18 @@ private:
bool validateJointNames() {
std::vector<std::string> actual_joint_names = robot_.getJointNames();
if (goal_.trajectory.joint_names.size() != actual_joint_names.size())
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::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;
@@ -317,19 +429,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<control_msgs::FollowJointTrajectoryAction>::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() {
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::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())
return false;
}
return true;
}
bool has_limited_velocities() {
for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) {
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::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;
}
@@ -338,12 +467,14 @@ private:
}
bool traj_is_finite() {
for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) {
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::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;
}
}
@@ -441,6 +572,7 @@ private:
}
void publishMbMsg() {
bool warned = false;
ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>(
"ur_driver/io_states", 1);
@@ -482,6 +614,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();
}