├── CMakeLists.txt ├── README.md ├── config ├── controllers.yaml ├── controllers.yaml~ ├── controllers_sim.yaml ├── fake_joint_offsets.yaml ├── hardware.yaml ├── joint_limits.yaml └── joint_offsets.yaml ├── include └── tr1_hardware_interface │ ├── tr1_hardware.h │ └── tr1_hardware_interface.h ├── launch ├── tr1_effort_controllers.launch ├── tr1_position_controllers.launch └── tr1_position_trajectory_controllers.launch ├── package.xml └── src ├── .tr1_hardware_interface.cpp.swp ├── tr1_hardware_interface.cpp └── tr1_hardware_interface_node.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(tr1_hardware_interface) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | hardware_interface 8 | controller_manager 9 | roscpp 10 | control_msgs 11 | trajectory_msgs 12 | actionlib 13 | pluginlib 14 | transmission_interface 15 | urdf 16 | control_toolbox 17 | joint_limits_interface 18 | tr1cpp 19 | ) 20 | 21 | catkin_package( 22 | INCLUDE_DIRS 23 | include 24 | CATKIN_DEPENDS 25 | hardware_interface 26 | controller_manager 27 | roscpp 28 | control_msgs 29 | trajectory_msgs 30 | pluginlib 31 | transmission_interface 32 | urdf 33 | control_toolbox 34 | joint_limits_interface 35 | tr1cpp 36 | LIBRARIES 37 | tr1_hardware_interface 38 | tr1cpp 39 | ) 40 | 41 | include_directories( 42 | include/ 43 | ${catkin_INCLUDE_DIRS}) 44 | 45 | install(DIRECTORY include/${PROJECT_NAME}/ 46 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 47 | FILES_MATCHING PATTERN "*.h" 48 | PATTERN ".svn" EXCLUDE) 49 | 50 | add_library(tr1_hardware_interface src/tr1_hardware_interface.cpp) 51 | add_dependencies(tr1_hardware_interface tr1cpp) 52 | target_link_libraries(tr1_hardware_interface 53 | ${tr1cpp_LIBRARIES} 54 | ${catkin_LIBRARIES}) 55 | 56 | add_executable(tr1_hardware_interface_node src/tr1_hardware_interface_node.cpp) 57 | add_dependencies(tr1_hardware_interface_node tr1_hardware_interface) 58 | 59 | target_link_libraries(tr1_hardware_interface_node 60 | tr1_hardware_interface 61 | ${catkin_LIBRARIES}) 62 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # tr1_hardware_interface 2 | -------------------------------------------------------------------------------- /config/controllers.yaml: -------------------------------------------------------------------------------- 1 | tr1: 2 | controller: 3 | state: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 50 6 | trajectory: 7 | RightPosition: 8 | type: effort_controllers/JointTrajectoryController 9 | joints: 10 | - JointRightShoulderPan 11 | - JointRightShoulderTilt 12 | - JointRightUpperArmRoll 13 | - JointRightElbowFlex 14 | - JointRightForearmRoll 15 | - JointRightWristFlex 16 | - JointRightWristRoll 17 | gains: 18 | JointRightShoulderPan: {p: 6.0, i: 0.0, d: 0.0, i_clamp: 1.0} 19 | JointRightShoulderTilt: {p: 15.0, i: 0.1, d: 1.0, i_clamp: 1.0} 20 | JointRightUpperArmRoll: {p: 10.0, i: 0.1, d: 1.0, i_clamp: 1.0} 21 | JointRightElbowFlex: {p: 10.0, i: 0.1, d: 1.0, i_clamp: 1.0} 22 | JointRightForearmRoll: {p: 0.8, i: 0.2, d: 0.0, i_clamp: 1.0} 23 | JointRightWristFlex: {p: 6.0, i: 0.0, d: 1.0, i_clamp: 1.0} 24 | JointRightWristRoll: {p: 1.0, i: 0.0, d: 0.0, i_clamp: 1.0} 25 | constraints: 26 | goal_time: 10.0 27 | JointRightShoulderPan: 28 | goal: 0.1 29 | JointRightShoulderTilt: 30 | goal: 0.1 31 | JointRightUpperArmRoll: 32 | goal: 0.1 33 | JointRightElbowFlex: 34 | goal: 0.1 35 | JointRightForearmRoll: 36 | goal: 0.1 37 | JointRightWristFlex: 38 | goal: 0.1 39 | JointRightWristRoll: 40 | goal: 0.1 41 | LeftPosition: 42 | type: effort_controllers/JointTrajectoryController 43 | joints: 44 | - JointLeftShoulderPan 45 | - JointLeftShoulderTilt 46 | - JointLeftUpperArmRoll 47 | - JointLeftElbowFlex 48 | - JointLeftForearmRoll 49 | - JointLeftWristFlex 50 | - JointLeftWristRoll 51 | gains: 52 | JointLeftShoulderPan: {p: 3.0, i: 0.3, d: 0.0, i_clamp: 1.0} 53 | JointLeftShoulderTilt: {p: 15.0, i: 0.0, d: 1.0, i_clamp: 0.0} 54 | JointLeftUpperArmRoll: {p: 15.0, i: 0.0, d: 1.0, i_clamp: 0.0} 55 | JointLeftElbowFlex: {p: 15.0, i: 0.0, d: 1.0, i_clamp: 0.0} 56 | JointLeftForearmRoll: {p: 3.0, i: 0.0, d: 1.0, i_clamp: 0.0} 57 | JointLeftWristFlex: {p: 3.0, i: 0.0, d: 1.0, i_clamp: 0.0} 58 | JointLeftWristRoll: {p: 3.0, i: 0.0, d: 1.0, i_clamp: 0.0} 59 | position: 60 | JointRightShoulderPan: 61 | type: effort_controllers/JointPositionController 62 | joint: JointRightShoulderPan 63 | pid: {p: 3.0, i: 0.3, d: 0.0, i_clamp: 1.0} 64 | JointRightShoulderTilt: 65 | type: effort_controllers/JointPositionController 66 | joint: JointRightShoulderTilt 67 | pid: {p: 15.0, i: 0.0, d: 1.0, i_clamp: 0.0} 68 | JointRightUpperArmRoll: 69 | type: effort_controllers/JointPositionController 70 | joint: JointRightUpperArmRoll 71 | pid: {p: 15.0, i: 0.0, d: 1.0, i_clamp: 0.0} 72 | JointRightElbowFlex: 73 | type: effort_controllers/JointPositionController 74 | joint: JointRightElbowFlex 75 | pid: {p: 15.0, i: 0.0, d: 1.0, i_clamp: 0.0} 76 | JointRightForearmRoll: 77 | type: effort_controllers/JointPositionController 78 | joint: JointRightForearmRoll 79 | pid: {p: 0.8, i: 0.2, d: 0.0, i_clamp: 1.0} 80 | JointRightWristFlex: 81 | type: effort_controllers/JointPositionController 82 | joint: JointRightWristFlex 83 | pid: {p: 3.0, i: 0.0, d: 1.0, i_clamp: 1.0} 84 | #pid: {p: 0.0, i: 0.0, d: 0.0} 85 | JointRightWristRoll: 86 | type: effort_controllers/JointPositionController 87 | joint: JointRightWristRoll 88 | pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp: 0.0} 89 | JointRightGripper: 90 | type: JointRightGripper/JointPositionController 91 | joint: JointRightWristFlex 92 | pid: {p: 3.0, i: 0.0, d: 1.0, i_clamp: 1.0} 93 | JointLeftShoulderPan: 94 | type: effort_controllers/JointPositionController 95 | joint: JointLeftShoulderPan 96 | pid: {p: 10.0, i: 0.0, d: 1.0} 97 | JointLeftShoulderTilt: 98 | type: effort_controllers/JointPositionController 99 | joint: JointLeftShoulderTilt 100 | pid: {p: 10.0, i: 0.0, d: 1.0} 101 | JointLeftUpperArmRoll: 102 | type: effort_controllers/JointPositionController 103 | joint: JointLeftUpperArmRoll 104 | pid: {p: 2.5, i: 0.0, d: 1.0} 105 | JointLeftElbowFlex: 106 | type: effort_controllers/JointPositionController 107 | joint: JointLeftElbowFlex 108 | pid: {p: 10.0, i: 0.0, d: 1.0} 109 | JointLeftForearmRoll: 110 | type: effort_controllers/JointPositionController 111 | joint: JointLeftForearmRoll 112 | pid: {p: 5.0, i: 0.0, d: 1.0} 113 | JointLeftWristFlex: 114 | type: effort_controllers/JointPositionController 115 | joint: JointLeftWristFlex 116 | pid: {p: 1.0, i: 0.0, d: 0.0} 117 | neck_to_head: 118 | type: effort_controllers/JointPositionController 119 | joint: neck_to_head 120 | pid: {p: 5.0, i: 0.0, d: 1.0} 121 | neck_base_to_neck: 122 | type: effort_controllers/JointPositionController 123 | joint: neck_base_to_neck 124 | pid: {p: 5.0, i: 0.0, d: 1.0} 125 | effort: 126 | JointTorsoExtension: 127 | type: effort_controllers/JointEffortController 128 | joint: JointTorsoExtension 129 | JointBaseWheelFR: 130 | type: effort_controllers/JointEffortController 131 | joint: JointBaseWheelFR 132 | JointBaseWheelFL: 133 | type: effort_controllers/JointEffortController 134 | joint: JointBaseWheelFL 135 | JointBaseWheelBR: 136 | type: effort_controllers/JointEffortController 137 | joint: JointBaseWheelBR 138 | JointBaseWheelBL: 139 | type: effort_controllers/JointEffortController 140 | joint: JointBaseWheelBL 141 | JointRightShoulderPan: 142 | type: effort_controllers/JointEffortController 143 | joint: JointRightShoulderPan 144 | JointRightShoulderTilt: 145 | type: effort_controllers/JointEffortController 146 | joint: JointRightShoulderTilt 147 | JointRightUpperArmRoll: 148 | type: effort_controllers/JointEffortController 149 | joint: JointRightUpperArmRoll 150 | JointRightElbowFlex: 151 | type: effort_controllers/JointEffortController 152 | joint: JointRightElbowFlex 153 | JointRightForearmRoll: 154 | type: effort_controllers/JointEffortController 155 | joint: JointRightForearmRoll 156 | JointRightWristFlex: 157 | type: effort_controllers/JointEffortController 158 | joint: JointRightWristFlex 159 | JointRightWristRoll: 160 | type: effort_controllers/JointEffortController 161 | joint: JointRightWristRoll 162 | JointRightGripper: 163 | type: effort_controllers/JointEffortController 164 | joint: JointRightGripper 165 | JointLeftShoulderPan: 166 | type: effort_controllers/JointEffortController 167 | joint: JointLeftShoulderPan 168 | JointLeftShoulderTilt: 169 | type: effort_controllers/JointEffortController 170 | joint: JointLeftShoulderTilt 171 | JointLeftUpperArmRoll: 172 | type: effort_controllers/JointEffortController 173 | joint: JointLeftUpperArmRoll 174 | JointLeftElbowFlex: 175 | type: effort_controllers/JointEffortController 176 | joint: JointLeftElbowFlex 177 | JointLeftForearmRoll: 178 | type: effort_controllers/JointEffortController 179 | joint: JointLeftForearmRoll 180 | JointLeftWristFlex: 181 | type: effort_controllers/JointEffortController 182 | joint: JointLeftWristFlex 183 | JointLeftWristRoll: 184 | type: effort_controllers/JointEffortController 185 | joint: JointLeftWristRoll 186 | JointLeftGripper: 187 | type: effort_controllers/JointEffortController 188 | joint: JointLeftGripper 189 | neck_to_head: 190 | type: effort_controllers/JointEffortController 191 | joint: neck_to_head 192 | pid: {p: 5.0, i: 0.0, d: 1.0} 193 | neck_base_to_neck: 194 | type: effort_controllers/JointEffortController 195 | joint: neck_base_to_neck 196 | pid: {p: 5.0, i: 0.0, d: 1.0} 197 | -------------------------------------------------------------------------------- /config/controllers.yaml~: -------------------------------------------------------------------------------- 1 | tr1: 2 | controller: 3 | state: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 50 6 | trajectory: 7 | RightPosition: 8 | type: effort_controllers/JointTrajectoryController 9 | joints: 10 | - JointRightShoulderPan 11 | - JointRightShoulderTilt 12 | - JointRightUpperArmRoll 13 | - JointRightElbowFlex 14 | - JointRightForearmRoll 15 | - JointRightWristFlex 16 | - JointRightWristRoll 17 | gains: 18 | JointRightShoulderPan: {p: 3.0, i: 0.3, d: 0.0, i_clamp: 1.0} 19 | JointRightShoulderTilt: {p: 15.0, i: 0.0, d: 1.0, i_clamp: 0.0} 20 | JointRightUpperArmRoll: {p: 15.0, i: 0.0, d: 1.0, i_clamp: 0.0} 21 | JointRightElbowFlex: {p: 15.0, i: 0.0, d: 1.0, i_clamp: 0.0} 22 | JointRightForearmRoll: {p: 0.0, i: 0.0, d: 0.0} 23 | JointRightWristFlex: {p: 0.0, i: 0.0, d: 0.0} 24 | JointRightWristRoll: {p: 0.0, i: 0.0, d: 0.0} 25 | LeftPosition: 26 | type: position_controllers/JointTrajectoryController 27 | joints: 28 | - JointLeftShoulderPan 29 | - JointLeftShoulderTilt 30 | - JointLeftUpperArmRoll 31 | - JointLeftElbowFlex 32 | - JointLeftForearmRoll 33 | - JointLeftWristFlex 34 | - JointLeftWristRoll 35 | gains: 36 | JointLeftShoulderPan: {p: 10.0, i: 0.0, d: 1.0} 37 | JointLeftShoulderTilt: {p: 10.0, i: 0.0, d: 1.0} 38 | JointLeftUpperArmRoll: {p: 2.5, i: 0.0, d: 1.0} 39 | JointLeftElbowFlex: {p: 10.0, i: 0.0, d: 1.0} 40 | JointLeftForearmRoll: {p: 5.0, i: 0.0, d: 1.0} 41 | JointLeftWristFlex: {p: 5.0, i: 0.0, d: 1.0} 42 | JointLeftWristRoll: {p: 5.0, i: 0.0, d: 1.0} 43 | position: 44 | JointRightShoulderPan: 45 | type: effort_controllers/JointPositionController 46 | joint: JointRightShoulderPan 47 | pid: {p: 3.0, i: 0.3, d: 0.0, i_clamp: 1.0} 48 | JointRightShoulderTilt: 49 | type: effort_controllers/JointPositionController 50 | joint: JointRightShoulderTilt 51 | pid: {p: 15.0, i: 0.0, d: 1.0, i_clamp: 0.0} 52 | JointRightUpperArmRoll: 53 | type: effort_controllers/JointPositionController 54 | joint: JointRightUpperArmRoll 55 | pid: {p: 15.0, i: 0.0, d: 1.0, i_clamp: 0.0} 56 | JointRightElbowFlex: 57 | type: effort_controllers/JointPositionController 58 | joint: JointRightElbowFlex 59 | pid: {p: 15.0, i: 0.0, d: 1.0, i_clamp: 0.0} 60 | JointRightForearmRoll: 61 | type: effort_controllers/JointPositionController 62 | joint: JointRightForearmRoll 63 | pid: {p: 3.0, i: 0.0, d: 1.0, i_clamp: 1.0} 64 | #pid: {p: 0.0, i: 0.0, d: 0.0} 65 | JointRightWristFlex: 66 | type: effort_controllers/JointPositionController 67 | joint: JointRightWristFlex 68 | pid: {p: 3.0, i: 0.0, d: 1.0, i_clamp: 1.0} 69 | #pid: {p: 0.0, i: 0.0, d: 0.0} 70 | JointLeftShoulderPan: 71 | type: effort_controllers/JointPositionController 72 | joint: JointLeftShoulderPan 73 | pid: {p: 10.0, i: 0.0, d: 1.0} 74 | JointLeftShoulderTilt: 75 | type: effort_controllers/JointPositionController 76 | joint: JointLeftShoulderTilt 77 | pid: {p: 10.0, i: 0.0, d: 1.0} 78 | JointLeftUpperArmRoll: 79 | type: effort_controllers/JointPositionController 80 | joint: JointLeftUpperArmRoll 81 | pid: {p: 2.5, i: 0.0, d: 1.0} 82 | JointLeftElbowFlex: 83 | type: effort_controllers/JointPositionController 84 | joint: JointLeftElbowFlex 85 | pid: {p: 10.0, i: 0.0, d: 1.0} 86 | JointLeftForearmRoll: 87 | type: effort_controllers/JointPositionController 88 | joint: JointLeftForearmRoll 89 | pid: {p: 5.0, i: 0.0, d: 1.0} 90 | JointLeftWristFlex: 91 | type: effort_controllers/JointPositionController 92 | joint: JointLeftWristFlex 93 | pid: {p: 5.0, i: 0.0, d: 1.0} 94 | effort: 95 | JointRightShoulderPan: 96 | type: effort_controllers/JointEffortController 97 | joint: JointRightShoulderPan 98 | JointRightShoulderTilt: 99 | type: effort_controllers/JointEffortController 100 | joint: JointRightShoulderTilt 101 | JointRightUpperArmRoll: 102 | type: effort_controllers/JointEffortController 103 | joint: JointRightUpperArmRoll 104 | JointRightElbowFlex: 105 | type: effort_controllers/JointEffortController 106 | joint: JointRightElbowFlex 107 | JointRightForearmRoll: 108 | type: effort_controllers/JointEffortController 109 | joint: JointRightForearmRoll 110 | JointRightWristFlex: 111 | type: effort_controllers/JointEffortController 112 | joint: JointRightWristFlex 113 | JointRightWristRoll: 114 | type: effort_controllers/JointEffortController 115 | joint: JointRightWristRoll 116 | JointRightGripper: 117 | type: effort_controllers/JointEffortController 118 | joint: JointRightGripper 119 | JointLeftShoulderPan: 120 | type: effort_controllers/JointEffortController 121 | joint: JointLeftShoulderPan 122 | JointLeftShoulderTilt: 123 | type: effort_controllers/JointEffortController 124 | joint: JointLeftShoulderTilt 125 | JointLeftUpperArmRoll: 126 | type: effort_controllers/JointEffortController 127 | joint: JointLeftUpperArmRoll 128 | JointLeftElbowFlex: 129 | type: effort_controllers/JointEffortController 130 | joint: JointLeftElbowFlex 131 | JointLeftForearmRoll: 132 | type: effort_controllers/JointEffortController 133 | joint: JointLeftForearmRoll 134 | JointLeftWristFlex: 135 | type: effort_controllers/JointEffortController 136 | joint: JointLeftWristFlex 137 | JointLeftWristRoll: 138 | type: effort_controllers/JointEffortController 139 | joint: JointLeftWristRoll 140 | JointLeftGripper: 141 | type: effort_controllers/JointEffortController 142 | joint: JointLeftGripper 143 | -------------------------------------------------------------------------------- /config/controllers_sim.yaml: -------------------------------------------------------------------------------- 1 | tr1: 2 | controller: 3 | state: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 50 6 | trajectory: 7 | RightPosition: 8 | type: effort_controllers/JointTrajectoryController 9 | joints: 10 | - JointRightShoulderPan 11 | - JointRightShoulderTilt 12 | - JointRightUpperArmRoll 13 | - JointRightElbowFlex 14 | - JointRightForearmRoll 15 | - JointRightWristFlex 16 | - JointRightWristRoll 17 | gains: 18 | JointRightShoulderPan: {p: 8.5, i: 0.3, d: 0.1, i_clamp: 1.0} 19 | JointRightShoulderTilt: {p: 15.0, i: 0.7, d: 1.0, i_clamp: 0.0} 20 | JointRightUpperArmRoll: {p: 15.0, i: 0.0, d: 1.0, i_clamp: 0.0} 21 | JointRightElbowFlex: {p: 15.0, i: 1.2, d: 1.0, i_clamp: 0.0} 22 | JointRightForearmRoll: {p: 1.5, i: 0.2, d: 0.0, i_clamp: 1.0} 23 | JointRightWristFlex: {p: 3.3, i: 0.0, d: 1.0, i_clamp: 1.0} 24 | JointRightWristRoll: {p: 1.0, i: 0.0, d: 0.0, i_clamp: 0.0} 25 | constraints: 26 | goal_time: 10.0 27 | JointRightShoulderPan: 28 | goal: 0.1 29 | JointRightShoulderTilt: 30 | goal: 0.1 31 | JointRightUpperArmRoll: 32 | goal: 0.1 33 | JointRightElbowFlex: 34 | goal: 0.1 35 | JointRightForearmRoll: 36 | goal: 0.1 37 | JointRightWristFlex: 38 | goal: 0.1 39 | JointRightWristRoll: 40 | goal: 0.1 41 | LeftPosition: 42 | type: effort_controllers/JointTrajectoryController 43 | joints: 44 | - JointLeftShoulderPan 45 | - JointLeftShoulderTilt 46 | - JointLeftUpperArmRoll 47 | - JointLeftElbowFlex 48 | - JointLeftForearmRoll 49 | - JointLeftWristFlex 50 | - JointLeftWristRoll 51 | gains: 52 | JointLeftShoulderPan: {p: 10.0, i: 0.0, d: 1.0, i_clamp: 0.0} 53 | JointLeftShoulderTilt: {p: 70.0, i: 2.0, d: 1.0, i_clamp: 1.0} 54 | JointLeftUpperArmRoll: {p: 10.5, i: 15.0, d: 1.0, i_clamp: 1.0} 55 | JointLeftElbowFlex: {p: 10.0, i: 3.0, d: 1.0, i_clamp: 0.0} 56 | JointLeftForearmRoll: {p: 5.0, i: 0.0, d: 1.0, i_clamp: 0.0} 57 | JointLeftWristFlex: {p: 3.0, i: 1.0, d: 0.0, i_clamp: 0.0} 58 | JointLeftWristRoll: {p: 1.0, i: 0.0, d: 0.0, i_clamp: 0.0} 59 | position: 60 | JointRightShoulderPan: 61 | type: effort_controllers/JointPositionController 62 | joint: JointRightShoulderPan 63 | pid: {p: 8.5, i: 0.3, d: 0.1, i_clamp: 1.0} 64 | JointRightShoulderTilt: 65 | type: effort_controllers/JointPositionController 66 | joint: JointRightShoulderTilt 67 | pid: {p: 15.0, i: 0.7, d: 1.0, i_clamp: 0.0} 68 | JointRightUpperArmRoll: 69 | type: effort_controllers/JointPositionController 70 | joint: JointRightUpperArmRoll 71 | pid: {p: 15.0, i: 0.0, d: 1.0, i_clamp: 0.0} 72 | JointRightElbowFlex: 73 | type: effort_controllers/JointPositionController 74 | joint: JointRightElbowFlex 75 | pid: {p: 15.0, i: 1.2, d: 1.0, i_clamp: 0.0} 76 | JointRightForearmRoll: 77 | type: effort_controllers/JointPositionController 78 | joint: JointRightForearmRoll 79 | pid: {p: 0.8, i: 0.2, d: 0.0, i_clamp: 1.0} 80 | JointRightWristFlex: 81 | type: effort_controllers/JointPositionController 82 | joint: JointRightWristFlex 83 | pid: {p: 3.3, i: 0.0, d: 1.0, i_clamp: 1.0} 84 | JointRightWristRoll: 85 | type: effort_controllers/JointPositionController 86 | joint: JointRightWristRoll 87 | pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp: 0.0} 88 | JointRightGripper: 89 | type: effort_controllers/JointPositionController 90 | joint: JointRightGripper 91 | pid: {p: 3.0, i: 0.0, d: 1.0, i_clamp: 1.0} 92 | JointLeftShoulderPan: 93 | type: effort_controllers/JointPositionController 94 | joint: JointLeftShoulderPan 95 | pid: {p: 10.0, i: 0.0, d: 1.0, i_clamp: 0.0} 96 | JointLeftShoulderTilt: 97 | type: effort_controllers/JointPositionController 98 | joint: JointLeftShoulderTilt 99 | pid: {p: 70.0, i: 2.0, d: 1.0, i_clamp: 1.0} 100 | JointLeftUpperArmRoll: 101 | type: effort_controllers/JointPositionController 102 | joint: JointLeftUpperArmRoll 103 | pid: {p: 10.5, i: 15.0, d: 1.0, i_clamp: 1.0} 104 | JointLeftElbowFlex: 105 | type: effort_controllers/JointPositionController 106 | joint: JointLeftElbowFlex 107 | pid: {p: 10.0, i: 3.0, d: 1.0, i_clamp: 0.0} 108 | JointLeftForearmRoll: 109 | type: effort_controllers/JointPositionController 110 | joint: JointLeftForearmRoll 111 | pid: {p: 5.0, i: 0.0, d: 1.0, i_clamp: 0.0} 112 | JointLeftWristFlex: 113 | type: effort_controllers/JointPositionController 114 | joint: JointLeftWristFlex 115 | pid: {p: 3.0, i: 1.0, d: 0.0, i_clamp: 0.0} 116 | JointLeftWristRoll: 117 | type: effort_controllers/JointPositionController 118 | joint: JointLeftWristRoll 119 | pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp: 0.0} 120 | neck_to_head: 121 | type: effort_controllers/JointPositionController 122 | joint: neck_to_head 123 | pid: {p: 5.0, i: 0.0, d: 1.0} 124 | neck_base_to_neck: 125 | type: effort_controllers/JointPositionController 126 | joint: neck_base_to_neck 127 | pid: {p: 5.0, i: 0.0, d: 1.0} 128 | effort: 129 | JointTorsoExtension: 130 | type: effort_controllers/JointEffortController 131 | joint: JointTorsoExtension 132 | JointBaseWheelFR: 133 | type: effort_controllers/JointEffortController 134 | joint: JointBaseWheelFR 135 | JointBaseWheelFL: 136 | type: effort_controllers/JointEffortController 137 | joint: JointBaseWheelFL 138 | JointBaseWheelBR: 139 | type: effort_controllers/JointEffortController 140 | joint: JointBaseWheelBR 141 | JointBaseWheelBL: 142 | type: effort_controllers/JointEffortController 143 | joint: JointBaseWheelBL 144 | JointRightShoulderPan: 145 | type: effort_controllers/JointEffortController 146 | joint: JointRightShoulderPan 147 | JointRightShoulderTilt: 148 | type: effort_controllers/JointEffortController 149 | joint: JointRightShoulderTilt 150 | JointRightUpperArmRoll: 151 | type: effort_controllers/JointEffortController 152 | joint: JointRightUpperArmRoll 153 | JointRightElbowFlex: 154 | type: effort_controllers/JointEffortController 155 | joint: JointRightElbowFlex 156 | JointRightForearmRoll: 157 | type: effort_controllers/JointEffortController 158 | joint: JointRightForearmRoll 159 | JointRightWristFlex: 160 | type: effort_controllers/JointEffortController 161 | joint: JointRightWristFlex 162 | JointRightWristRoll: 163 | type: effort_controllers/JointEffortController 164 | joint: JointRightWristRoll 165 | JointRightGripper: 166 | type: effort_controllers/JointEffortController 167 | joint: JointRightGripper 168 | JointLeftShoulderPan: 169 | type: effort_controllers/JointEffortController 170 | joint: JointLeftShoulderPan 171 | JointLeftShoulderTilt: 172 | type: effort_controllers/JointEffortController 173 | joint: JointLeftShoulderTilt 174 | JointLeftUpperArmRoll: 175 | type: effort_controllers/JointEffortController 176 | joint: JointLeftUpperArmRoll 177 | JointLeftElbowFlex: 178 | type: effort_controllers/JointEffortController 179 | joint: JointLeftElbowFlex 180 | JointLeftForearmRoll: 181 | type: effort_controllers/JointEffortController 182 | joint: JointLeftForearmRoll 183 | JointLeftWristFlex: 184 | type: effort_controllers/JointEffortController 185 | joint: JointLeftWristFlex 186 | JointLeftWristRoll: 187 | type: effort_controllers/JointEffortController 188 | joint: JointLeftWristRoll 189 | JointLeftGripper: 190 | type: effort_controllers/JointEffortController 191 | joint: JointLeftGripper 192 | neck_to_head: 193 | type: effort_controllers/JointEffortController 194 | joint: neck_to_head 195 | pid: {p: 5.0, i: 0.0, d: 1.0} 196 | neck_base_to_neck: 197 | type: effort_controllers/JointEffortController 198 | joint: neck_base_to_neck 199 | pid: {p: 5.0, i: 0.0, d: 1.0} 200 | -------------------------------------------------------------------------------- /config/fake_joint_offsets.yaml: -------------------------------------------------------------------------------- 1 | tr1: 2 | joint_offsets: 3 | JointRightShoulderPan: 0.0 4 | JointRightShoulderTilt: 0.0 5 | JointRightUpperArmRoll: 0.0 6 | JointRightElbowFlex: 0.0 7 | JointRightForearmRoll: 0.0 8 | JointRightWristFlex: 0.0 9 | JointRightWristRoll: 0.0 10 | JointRightGripper: 0.0 11 | JointLeftShoulderPan: 0.0 12 | JointLeftShoulderTilt: 0.0 13 | JointLeftUpperArmRoll: 0.0 14 | JointLeftElbowFlex: 0.0 15 | JointLeftForearmRoll: 0.0 16 | JointLeftWristFlex: 0.0 17 | JointLeftWristRoll: 0.0 18 | JointLeftGripper: 0.0 19 | joint_read_ratio: 20 | JointRightShoulderPan: 1.0 21 | JointRightShoulderTilt: 1.0 22 | JointRightUpperArmRoll: 1.0 23 | JointRightElbowFlex: 1.0 24 | JointRightForearmRoll: 1.0 25 | JointRightWristFlex: 1.0 26 | JointRightWristRoll: 1.0 27 | JointRightGripper: 1.0 28 | JointLeftShoulderPan: 1.0 29 | JointLeftShoulderTilt: 1.0 30 | JointLeftUpperArmRoll: 1.0 31 | JointLeftElbowFlex: 1.0 32 | JointLeftForearmRoll: 1.0 33 | JointLeftWristFlex: 1.0 34 | JointLeftWristRoll: 1.0 35 | JointLeftGripper: 1.0 36 | -------------------------------------------------------------------------------- /config/hardware.yaml: -------------------------------------------------------------------------------- 1 | tr1: 2 | hardware_interface: 3 | loop_hz: 10 # hz 4 | joints: 5 | - JointRightShoulderPan 6 | - JointRightShoulderTilt 7 | - JointRightUpperArmRoll 8 | - JointRightElbowFlex 9 | - JointRightForearmRoll 10 | - JointRightWristFlex 11 | - JointRightWristRoll 12 | - JointRightGripper 13 | - JointLeftShoulderPan 14 | - JointLeftShoulderTilt 15 | - JointLeftUpperArmRoll 16 | - JointLeftElbowFlex 17 | - JointLeftForearmRoll 18 | - JointLeftWristFlex 19 | - JointLeftWristRoll 20 | - JointLeftGripper 21 | - JointBaseWheelFL 22 | - JointBaseWheelFR 23 | - JointBaseWheelBL 24 | - JointBaseWheelBR 25 | - neck_base_to_neck 26 | - neck_to_head 27 | - JointTorsoExtension 28 | -------------------------------------------------------------------------------- /config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | #tr1: 2 | joint_limits: 3 | 4 | JointRightShoulderPan: 5 | has_position_limits: true 6 | min_position: -1.0 7 | max_position: 1.5708 8 | has_velocity_limits: true 9 | max_velocity: 0.2932 10 | has_acceleration_limits: false 11 | max_acceleration: 0.0 12 | has_jerk_limits: true 13 | max_jerk: 100.0 14 | has_effort_limits: true 15 | max_effort: 1.0 16 | min_effort: -1.0 17 | 18 | JointRightShoulderTilt: 19 | has_position_limits: true 20 | min_position: -1.4 21 | max_position: 1.5708 22 | has_velocity_limits: true 23 | max_velocity: 0.1 24 | has_acceleration_limits: false 25 | max_acceleration: 0.0 26 | has_jerk_limits: true 27 | max_jerk: 100.0 28 | has_effort_limits: true 29 | max_effort: 1.0 30 | min_effort: -1.0 31 | 32 | JointRightUpperArmRoll: 33 | has_position_limits: true 34 | min_position: -3.0000 35 | max_position: 3.0000 36 | has_velocity_limits: true 37 | max_velocity: 0.25 38 | has_acceleration_limits: false 39 | max_acceleration: 0.0 40 | has_jerk_limits: true 41 | max_jerk: 100.0 42 | has_effort_limits: true 43 | max_effort: 1.0 44 | min_effort: -1.0 45 | 46 | JointRightElbowFlex: 47 | has_position_limits: true 48 | min_position: -1.5708 49 | max_position: 1.5708 50 | has_velocity_limits: true 51 | max_velocity: 0.1 52 | has_acceleration_limits: false 53 | max_acceleration: 0.0 54 | has_jerk_limits: true 55 | max_jerk: 100.0 56 | has_effort_limits: true 57 | max_effort: 1.0 58 | min_effort: -1.0 59 | 60 | JointRightForearmRoll: 61 | has_position_limits: true 62 | min_position: -3.0000 63 | max_position: 3.0000 64 | has_velocity_limits: true 65 | max_velocity: 1.4661 66 | has_acceleration_limits: false 67 | max_acceleration: 0.0 68 | has_jerk_limits: true 69 | max_jerk: 100.0 70 | has_effort_limits: true 71 | max_effort: 1.0 72 | min_effort: -1.0 73 | 74 | JointRightWristFlex: 75 | has_position_limits: true 76 | min_position: -1.75 77 | max_position: 1.75 78 | has_velocity_limits: true 79 | max_velocity: 0.7330 80 | has_acceleration_limits: false 81 | max_acceleration: 0.0 82 | has_jerk_limits: true 83 | max_jerk: 100.0 84 | has_effort_limits: true 85 | max_effort: 1.0 86 | min_effort: -1.0 87 | 88 | JointRightWristRoll: 89 | has_position_limits: true 90 | min_position: -2 91 | max_position: 2 92 | has_velocity_limits: true 93 | max_velocity: 7.5398 94 | has_acceleration_limits: false 95 | max_acceleration: 0.0 96 | has_jerk_limits: true 97 | max_jerk: 100.0 98 | has_effort_limits: true 99 | max_effort: 1.5708 100 | min_effort: -1.5708 101 | 102 | JointRightGripper: 103 | has_position_limits: true 104 | min_position: 0 105 | max_position: 1.5708 106 | has_velocity_limits: true 107 | max_velocity: 2.0 108 | has_acceleration_limits: false 109 | max_acceleration: 0.0 110 | has_jerk_limits: true 111 | max_jerk: 100.0 112 | has_effort_limits: true 113 | max_effort: 1.0 114 | min_effort: -1.0 115 | 116 | JointLeftShoulderPan: 117 | has_position_limits: true 118 | min_position: -1.0 119 | max_position: 1.5708 120 | has_velocity_limits: true 121 | max_velocity: 0.2932 122 | has_acceleration_limits: false 123 | max_acceleration: 0.0 124 | has_jerk_limits: true 125 | max_jerk: 100.0 126 | has_effort_limits: true 127 | max_effort: 1.0 128 | min_effort: -1.0 129 | 130 | JointLeftShoulderTilt: 131 | has_position_limits: true 132 | min_position: -1.5708 133 | max_position: 1.5708 134 | has_velocity_limits: true 135 | max_velocity: 0.1 136 | has_acceleration_limits: false 137 | max_acceleration: 0.0 138 | has_jerk_limits: true 139 | max_jerk: 100.0 140 | has_effort_limits: true 141 | max_effort: 1.0 142 | min_effort: -1.0 143 | 144 | JointLeftUpperArmRoll: 145 | has_position_limits: true 146 | min_position: -3.0000 147 | max_position: 3.0000 148 | has_velocity_limits: true 149 | max_velocity: 0.25 150 | has_acceleration_limits: false 151 | max_acceleration: 0.0 152 | has_jerk_limits: true 153 | max_jerk: 100.0 154 | has_effort_limits: true 155 | max_effort: 1.0 156 | min_effort: -1.0 157 | 158 | JointLeftElbowFlex: 159 | has_position_limits: true 160 | min_position: -1.75 161 | max_position: 1.75 162 | has_velocity_limits: true 163 | max_velocity: 0.1 164 | has_acceleration_limits: false 165 | max_acceleration: 0.0 166 | has_jerk_limits: true 167 | max_jerk: 100.0 168 | has_effort_limits: true 169 | max_effort: 1.0 170 | min_effort: -1.0 171 | 172 | JointLeftForearmRoll: 173 | has_position_limits: true 174 | min_position: -3.0000 175 | max_position: 3.0000 176 | has_velocity_limits: true 177 | max_velocity: 1.4661 178 | has_acceleration_limits: false 179 | max_acceleration: 0.0 180 | has_jerk_limits: true 181 | max_jerk: 100.0 182 | has_effort_limits: true 183 | max_effort: 1.0 184 | min_effort: -1.0 185 | 186 | JointLeftWristFlex: 187 | has_position_limits: true 188 | min_position: -1.75 189 | max_position: 1.75 190 | has_velocity_limits: true 191 | max_velocity: 0.7330 192 | has_acceleration_limits: false 193 | max_acceleration: 0.0 194 | has_jerk_limits: true 195 | max_jerk: 100.0 196 | has_effort_limits: true 197 | max_effort: 1.0 198 | min_effort: -1.0 199 | 200 | JointLeftWristRoll: 201 | has_position_limits: true 202 | min_position: -2 203 | max_position: 2 204 | has_velocity_limits: true 205 | max_velocity: 7.5398 206 | has_acceleration_limits: false 207 | max_acceleration: 0.0 208 | has_jerk_limits: true 209 | max_jerk: 100.0 210 | has_effort_limits: true 211 | max_effort: 1.0 212 | min_effort: -1.0 213 | 214 | JointLeftGripper: 215 | has_position_limits: true 216 | min_position: 0 217 | max_position: 1.5708 218 | has_velocity_limits: true 219 | max_velocity: 2.0 220 | has_acceleration_limits: false 221 | max_acceleration: 0.0 222 | has_jerk_limits: true 223 | max_jerk: 100.0 224 | has_effort_limits: true 225 | max_effort: 1.0 226 | min_effort: -1.0 227 | -------------------------------------------------------------------------------- /config/joint_offsets.yaml: -------------------------------------------------------------------------------- 1 | tr1: 2 | joint_offsets: 3 | JointBaseWheelBL: -395.69954811985457 4 | JointBaseWheelBR: -0.030679615757714843 5 | JointBaseWheelFL: -395.8345384291884 6 | JointBaseWheelFR: -0.01227184630308594 7 | JointLeftElbowFlex: 2.791845033952051 8 | JointLeftForearmRoll: -2.97592272849834 9 | JointLeftGripper: -0.0 10 | JointLeftShoulderPan: -1.568751019077819 11 | JointLeftShoulderTilt: -2.822524649709765 12 | JointLeftUpperArmRoll: 0.024543692606171952 13 | JointLeftWristFlex: 1.1965050145508789 14 | JointLeftWristRoll: -0.0 15 | JointRightElbowFlex: -2.2518837966162697 16 | JointRightForearmRoll: -1.329450016167643 17 | JointRightGripper: -0.0 18 | JointRightShoulderPan: -0.02863430804053385 19 | JointRightShoulderTilt: 1.0124273200045897 20 | JointRightUpperArmRoll: 1.0737865515200191 21 | JointRightWristFlex: 2.4748223377889977 22 | JointRightWristRoll: -0.0 23 | JointTorsoExtension: -0.0 24 | base_to_body: -0.0 25 | base_to_wheel1: -0.0 26 | base_to_wheel2: -0.0 27 | base_to_wheel3: -0.0 28 | base_to_wheel4: -0.0 29 | neck_base_to_neck: -1.5217089415826563 30 | neck_to_head: 0.049087385212343904 31 | joint_read_ratio: 32 | JointLeftElbowFlex: 0.944444444 33 | JointLeftForearmRoll: 1.076923077 34 | JointLeftGripper: 1.0 35 | JointLeftShoulderPan: 1.0 36 | JointLeftShoulderTilt: 1.0 37 | JointLeftUpperArmRoll: 1.076923077 38 | JointLeftWristFlex: 0.923076923 39 | JointLeftWristRoll: 1.0 40 | JointRightElbowFlex: 0.944444444 41 | JointRightForearmRoll: 1.076923077 42 | JointRightGripper: 1.0 43 | JointRightShoulderPan: 1.0 44 | JointRightShoulderTilt: 1.0 45 | JointRightUpperArmRoll: 1.076923077 46 | JointRightWristFlex: 0.923076923 47 | JointRightWristRoll: 1.0 48 | -------------------------------------------------------------------------------- /include/tr1_hardware_interface/tr1_hardware.h: -------------------------------------------------------------------------------- 1 | #ifndef ROS_CONTROL__TR1_HARDWARE_H 2 | #define ROS_CONTROL__TR1_HARDWARE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace tr1_hardware_interface 16 | { 17 | /// \brief Hardware interface for a robot 18 | class TR1Hardware : public hardware_interface::RobotHW 19 | { 20 | /*public: 21 | /// \brief Constructor. 22 | TR1Hardware(); 23 | /// \brief Destructor. 24 | ~TR1Hardware(); 25 | */ 26 | protected: 27 | 28 | // Interfaces 29 | hardware_interface::JointStateInterface joint_state_interface_; 30 | hardware_interface::PositionJointInterface position_joint_interface_; 31 | hardware_interface::VelocityJointInterface velocity_joint_interface_; 32 | hardware_interface::EffortJointInterface effort_joint_interface_; 33 | 34 | joint_limits_interface::EffortJointSaturationInterface effort_joint_saturation_interface_; 35 | joint_limits_interface::EffortJointSoftLimitsInterface effort_joint_limits_interface_; 36 | joint_limits_interface::PositionJointSaturationInterface position_joint_saturation_interface_; 37 | joint_limits_interface::PositionJointSoftLimitsInterface position_joint_limits_interface_; 38 | joint_limits_interface::VelocityJointSaturationInterface velocity_joint_saturation_interface_; 39 | joint_limits_interface::VelocityJointSoftLimitsInterface velocity_joint_limits_interface_; 40 | 41 | // Custom or available transmissions 42 | // transmission_interface::RRBOTTransmission rrbot_trans_; 43 | // std::vector simple_trans_; 44 | 45 | // Shared memory 46 | int num_joints_; 47 | int joint_mode_; // position, velocity, or effort 48 | std::vector joint_names_; 49 | std::vector joint_types_; 50 | std::vector joint_position_; 51 | std::vector joint_velocity_; 52 | std::vector joint_effort_; 53 | std::vector joint_position_command_; 54 | std::vector joint_velocity_command_; 55 | std::vector joint_effort_command_; 56 | std::vector joint_lower_limits_; 57 | std::vector joint_upper_limits_; 58 | std::vector joint_effort_limits_; 59 | 60 | }; // class 61 | 62 | } // namespace 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /include/tr1_hardware_interface/tr1_hardware_interface.h: -------------------------------------------------------------------------------- 1 | #ifndef ROS_CONTROL__TR1_HARDWARE_INTERFACE_H 2 | #define ROS_CONTROL__TR1_HARDWARE_INTERFACE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | using namespace hardware_interface; 20 | using joint_limits_interface::JointLimits; 21 | using joint_limits_interface::SoftJointLimits; 22 | using joint_limits_interface::PositionJointSoftLimitsHandle; 23 | using joint_limits_interface::PositionJointSoftLimitsInterface; 24 | 25 | 26 | namespace tr1_hardware_interface 27 | { 28 | static const double POSITION_STEP_FACTOR = 10; 29 | static const double VELOCITY_STEP_FACTOR = 10; 30 | 31 | class TR1HardwareInterface: public tr1_hardware_interface::TR1Hardware 32 | { 33 | public: 34 | TR1HardwareInterface(ros::NodeHandle& nh); 35 | ~TR1HardwareInterface(); 36 | void init(); 37 | void update(const ros::TimerEvent& e); 38 | void read(); 39 | void write(ros::Duration elapsed_time); 40 | 41 | protected: 42 | tr1cpp::TR1 tr1; 43 | ros::NodeHandle nh_; 44 | ros::Timer non_realtime_loop_; 45 | ros::Duration control_period_; 46 | ros::Duration elapsed_time_; 47 | PositionJointInterface positionJointInterface; 48 | PositionJointSoftLimitsInterface positionJointSoftLimitsInterface; 49 | double loop_hz_; 50 | boost::shared_ptr controller_manager_; 51 | double p_error_, v_error_, e_error_; 52 | std::string _logInfo; 53 | }; 54 | 55 | } 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /launch/tr1_effort_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /launch/tr1_position_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /launch/tr1_position_trajectory_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | tr1_hardware_interface 4 | 0.0.1 5 | Provides the hardware definition and interfaces for TR1 6 | 7 | Zach Allen 8 | 9 | MIT 10 | 11 | 12 | 13 | / 14 | 15 | Zach Allen 16 | 17 | catkin 18 | 19 | hardware_interface 20 | controller_manager 21 | roscpp 22 | control_msgs 23 | trajectory_msgs 24 | actionlib 25 | pluginlib 26 | transmission_interface 27 | urdf 28 | control_toolbox 29 | joint_limits_interface 30 | tr1cpp 31 | 32 | hardware_interface 33 | controller_manager 34 | ros_controllers 35 | tr1_description 36 | roscpp 37 | control_msgs 38 | trajectory_msgs 39 | actionlib 40 | pluginlib 41 | transmission_interface 42 | urdf 43 | control_toolbox 44 | joint_limits_interface 45 | tr1cpp 46 | 47 | 48 | -------------------------------------------------------------------------------- /src/.tr1_hardware_interface.cpp.swp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SlateRobotics/tr1_hardware_interface/240c1f454f059fb0b83b0fd5dcc34d43de7e4a4c/src/.tr1_hardware_interface.cpp.swp -------------------------------------------------------------------------------- /src/tr1_hardware_interface.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace hardware_interface; 11 | using joint_limits_interface::JointLimits; 12 | using joint_limits_interface::SoftJointLimits; 13 | using joint_limits_interface::PositionJointSoftLimitsHandle; 14 | using joint_limits_interface::PositionJointSoftLimitsInterface; 15 | 16 | namespace tr1_hardware_interface 17 | { 18 | TR1HardwareInterface::TR1HardwareInterface(ros::NodeHandle& nh) \ 19 | : nh_(nh) 20 | { 21 | init(); 22 | controller_manager_.reset(new controller_manager::ControllerManager(this, nh_)); 23 | 24 | nh_.param("/tr1/hardware_interface/loop_hz", loop_hz_, 0.1); 25 | ROS_DEBUG_STREAM_NAMED("constructor","Using loop freqency of " << loop_hz_ << " hz"); 26 | ros::Duration update_freq = ros::Duration(1.0/loop_hz_); 27 | non_realtime_loop_ = nh_.createTimer(update_freq, &TR1HardwareInterface::update, this); 28 | 29 | ROS_INFO_NAMED("hardware_interface", "Loaded generic_hardware_interface."); 30 | } 31 | 32 | TR1HardwareInterface::~TR1HardwareInterface() 33 | { 34 | } 35 | 36 | void TR1HardwareInterface::init() 37 | { 38 | //joint_mode_ = 3; // ONLY EFFORT FOR NOW 39 | // Get joint names 40 | nh_.getParam("/tr1/hardware_interface/joints", joint_names_); 41 | if (joint_names_.size() == 0) 42 | { 43 | ROS_FATAL_STREAM_NAMED("init","No joints found on parameter server for controller. Did you load the proper yaml file?"); 44 | } 45 | num_joints_ = joint_names_.size(); 46 | 47 | // Resize vectors 48 | joint_position_.resize(num_joints_); 49 | joint_velocity_.resize(num_joints_); 50 | joint_effort_.resize(num_joints_); 51 | joint_position_command_.resize(num_joints_); 52 | joint_velocity_command_.resize(num_joints_); 53 | joint_effort_command_.resize(num_joints_); 54 | 55 | 56 | // Initialize controller 57 | for (int i = 0; i < num_joints_; ++i) 58 | { 59 | tr1cpp::Joint joint = tr1.getJoint(joint_names_[i]); 60 | 61 | ROS_DEBUG_STREAM_NAMED("constructor","Loading joint name: " << joint.name); 62 | 63 | nh_.getParam("/tr1/joint_offsets/" + joint.name, joint.angleOffset); 64 | nh_.getParam("/tr1/joint_read_ratio/" + joint.name, joint.readRatio); 65 | tr1.setJoint(joint); 66 | 67 | // Create joint state interface 68 | JointStateHandle jointStateHandle(joint.name, &joint_position_[i], &joint_velocity_[i], &joint_effort_[i]); 69 | joint_state_interface_.registerHandle(jointStateHandle); 70 | 71 | // Create position joint interface 72 | JointHandle jointPositionHandle(jointStateHandle, &joint_position_command_[i]); 73 | JointLimits limits; 74 | SoftJointLimits softLimits; 75 | if (getJointLimits(joint.name, nh_, limits) == false) { 76 | ROS_ERROR_STREAM("Cannot set joint limits for " << joint.name); 77 | } else { 78 | PositionJointSoftLimitsHandle jointLimitsHandle(jointPositionHandle, limits, softLimits); 79 | positionJointSoftLimitsInterface.registerHandle(jointLimitsHandle); 80 | } 81 | position_joint_interface_.registerHandle(jointPositionHandle); 82 | 83 | // Create velocity joint interface 84 | //JointHandle jointVelocityHandle(jointStateHandle, &joint_velocity_command_[i]); 85 | //effort_joint_interface_.registerHandle(jointVelocityHandle); 86 | 87 | // Create effort joint interface 88 | JointHandle jointEffortHandle(jointStateHandle, &joint_effort_command_[i]); 89 | effort_joint_interface_.registerHandle(jointEffortHandle); 90 | 91 | } 92 | 93 | registerInterface(&joint_state_interface_); 94 | registerInterface(&position_joint_interface_); 95 | //registerInterface(&velocity_joint_interface_); 96 | registerInterface(&effort_joint_interface_); 97 | registerInterface(&positionJointSoftLimitsInterface); 98 | } 99 | 100 | void TR1HardwareInterface::update(const ros::TimerEvent& e) 101 | { 102 | _logInfo = "\n"; 103 | _logInfo += "Joint Position Command:\n"; 104 | for (int i = 0; i < num_joints_; i++) 105 | { 106 | std::ostringstream jointPositionStr; 107 | jointPositionStr << joint_position_command_[i]; 108 | _logInfo += " " + joint_names_[i] + ": " + jointPositionStr.str() + "\n"; 109 | } 110 | 111 | elapsed_time_ = ros::Duration(e.current_real - e.last_real); 112 | 113 | read(); 114 | controller_manager_->update(ros::Time::now(), elapsed_time_); 115 | write(elapsed_time_); 116 | 117 | //ROS_INFO_STREAM(_logInfo); 118 | } 119 | 120 | void TR1HardwareInterface::read() 121 | { 122 | _logInfo += "Joint State:\n"; 123 | for (int i = 0; i < num_joints_; i++) 124 | { 125 | tr1cpp::Joint joint = tr1.getJoint(joint_names_[i]); 126 | 127 | if (joint.getActuatorType() == ACTUATOR_TYPE_MOTOR) 128 | { 129 | joint_position_[i] = joint.readAngle(); 130 | 131 | std::ostringstream jointPositionStr; 132 | jointPositionStr << joint_position_[i]; 133 | _logInfo += " " + joint.name + ": " + jointPositionStr.str() + "\n"; 134 | } 135 | 136 | tr1.setJoint(joint); 137 | } 138 | } 139 | 140 | void TR1HardwareInterface::write(ros::Duration elapsed_time) 141 | { 142 | positionJointSoftLimitsInterface.enforceLimits(elapsed_time); 143 | 144 | _logInfo += "Joint Effort Command:\n"; 145 | for (int i = 0; i < num_joints_; i++) 146 | { 147 | tr1cpp::Joint joint = tr1.getJoint(joint_names_[i]); 148 | //if (joint_effort_command_[i] > 1) joint_effort_command_[i] = 1; 149 | //if (joint_effort_command_[i] < -1) joint_effort_command_[i] = -1; 150 | 151 | double effort = joint_effort_command_[i]; 152 | uint8_t duration = 15; 153 | 154 | if (joint.getActuatorType() == 1) { // servo 155 | double previousEffort = joint.getPreviousEffort(); 156 | effort += previousEffort; 157 | } 158 | 159 | joint.actuate(effort, duration); 160 | 161 | std::ostringstream jointEffortStr; 162 | jointEffortStr << joint_effort_command_[i]; 163 | _logInfo += " " + joint.name + ": " + jointEffortStr.str() + "\n"; 164 | } 165 | } 166 | } 167 | 168 | -------------------------------------------------------------------------------- /src/tr1_hardware_interface_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char** argv) 4 | { 5 | ros::init(argc, argv, "tr1_hardware_interface"); 6 | ros::NodeHandle nh; 7 | 8 | // NOTE: We run the ROS loop in a separate thread as external calls such 9 | // as service callbacks to load controllers can block the (main) control loop 10 | ros::AsyncSpinner spinner(1); 11 | spinner.start(); 12 | 13 | tr1_hardware_interface::TR1HardwareInterface tr1(nh); 14 | 15 | ros::spin(); 16 | 17 | return 0; 18 | } 19 | --------------------------------------------------------------------------------