diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 00000000..26d33521 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,3 @@ +# Default ignored files +/shelf/ +/workspace.xml diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 00000000..105ce2da --- /dev/null +++ b/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 00000000..94a25f7f --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/README.md b/README.md index af5f0594..f3421da8 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,13 @@ +# Added ROS Driver for Arm + +Services added to control joints of the arm and the gripper. +https://github.com/estherRay/spot_ros/blob/master/spot_arm.mp4 + +# URDF and MoveIt simulation of Spot's arm + +Available in other repo: https://github.com/estherRay/Spot-Arm + + # Spot ROS Driver ![CP Spot](cp_spot.jpg) diff --git a/Spot-Arm/ArmDescription.png b/Spot-Arm/ArmDescription.png new file mode 100644 index 00000000..dfd6f0d4 Binary files /dev/null and b/Spot-Arm/ArmDescription.png differ diff --git a/Spot-Arm/ArmMoveIt.png b/Spot-Arm/ArmMoveIt.png new file mode 100644 index 00000000..2ea1c5c9 Binary files /dev/null and b/Spot-Arm/ArmMoveIt.png differ diff --git a/Spot-Arm/README.md b/Spot-Arm/README.md new file mode 100644 index 00000000..d65eb50d --- /dev/null +++ b/Spot-Arm/README.md @@ -0,0 +1,17 @@ +This repository was developed and tested on Ubuntu 18.04 with ROS Melodic. + + +spot_arm_description: + + Hardware description of spot arm (stl, urdf, ...) + ![alt text](https://github.com/estherRay/Spot-Arm/blob/main/ArmDescription.png) + + +spot_moveit_config: + + Moveit controller for motion planning + + Control of the arm using Rviz + + Gazebo simulation + ![alt text](https://github.com/estherRay/Spot-Arm/blob/main/ArmMoveIt.png) diff --git a/Spot-Arm/spot_arm_description/CMakeLists.txt b/Spot-Arm/spot_arm_description/CMakeLists.txt new file mode 100644 index 00000000..85fa8723 --- /dev/null +++ b/Spot-Arm/spot_arm_description/CMakeLists.txt @@ -0,0 +1,202 @@ +cmake_minimum_required(VERSION 3.0.2) +project(spot_arm_description) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES spot_arm_description +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include +# ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/spot_arm_description.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/spot_arm_description_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## 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 +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_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 +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_spot_arm_description.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Spot-Arm/spot_arm_description/launch/controller.launch b/Spot-Arm/spot_arm_description/launch/controller.launch new file mode 100644 index 00000000..63fc10f8 --- /dev/null +++ b/Spot-Arm/spot_arm_description/launch/controller.launch @@ -0,0 +1,9 @@ + + + + + + + + diff --git a/Spot-Arm/spot_arm_description/launch/controller.yaml b/Spot-Arm/spot_arm_description/launch/controller.yaml new file mode 100644 index 00000000..4549e142 --- /dev/null +++ b/Spot-Arm/spot_arm_description/launch/controller.yaml @@ -0,0 +1,35 @@ +spot_arm_controller: + # Publish all joint states ----------------------------------- + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + + # Position Controllers -------------------------------------- + Joint1_position_controller: + type: effort_controllers/JointPositionController + joint: Joint1 + pid: {p: 100.0, i: 0.01, d: 10.0} + Joint2_position_controller: + type: effort_controllers/JointPositionController + joint: Joint2 + pid: {p: 100.0, i: 0.01, d: 10.0} + Joint3_position_controller: + type: effort_controllers/JointPositionController + joint: Joint3 + pid: {p: 100.0, i: 0.01, d: 10.0} + Joint4_position_controller: + type: effort_controllers/JointPositionController + joint: Joint4 + pid: {p: 100.0, i: 0.01, d: 10.0} + Joint5_position_controller: + type: effort_controllers/JointPositionController + joint: Joint5 + pid: {p: 100.0, i: 0.01, d: 10.0} + Joint6_position_controller: + type: effort_controllers/JointPositionController + joint: Rev6 + pid: {p: 100.0, i: 0.01, d: 10.0} + Joint7_position_controller: + type: effort_controllers/JointPositionController + joint: Rev7 + pid: {p: 100.0, i: 0.01, d: 10.0} diff --git a/Spot-Arm/spot_arm_description/launch/display.launch b/Spot-Arm/spot_arm_description/launch/display.launch new file mode 100644 index 00000000..78f207f5 --- /dev/null +++ b/Spot-Arm/spot_arm_description/launch/display.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/Spot-Arm/spot_arm_description/launch/gazebo.launch b/Spot-Arm/spot_arm_description/launch/gazebo.launch new file mode 100644 index 00000000..3a9f1378 --- /dev/null +++ b/Spot-Arm/spot_arm_description/launch/gazebo.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/Spot-Arm/spot_arm_description/launch/urdf.rviz b/Spot-Arm/spot_arm_description/launch/urdf.rviz new file mode 100644 index 00000000..b80bcfad --- /dev/null +++ b/Spot-Arm/spot_arm_description/launch/urdf.rviz @@ -0,0 +1,422 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /TF1 + Splitter Ratio: 0.5 + Tree Height: 591 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + 2dofknuckle_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + 2dofknuckle_link_10: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + 2dofknuckle_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + 2dofknuckle_link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + 2dofknuckle_link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + 2dofknuckle_link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + 2dofknuckle_link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + 2dofknuckle_link_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + 2dofknuckle_link_8: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + 2dofknuckle_link_9: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bucket_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + face_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + facebracket_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hcsr04_cans_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hcsr04_chips_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hcsr04_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + leg_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + leg_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + leg_link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + leg_link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lipo_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + mg90_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + mg90_link_10: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + mg90_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + mg90_link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + mg90_link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + mg90_link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + mg90_link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + mg90_link_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + mg90_link_8: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + mg90_link_9: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + mpu9250_cap_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + mpu9250_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + mpu9250_mpu_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pizerow_cam_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pizerow_con_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pizerow_cpu_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pizerow_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + raspicam_cam_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + raspicam_chips_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + raspicam_con_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + raspicam_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + servobottom_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + servobottom_link_10: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + servobottom_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + servobottom_link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + servobottom_link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + servobottom_link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + servobottom_link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + servobottom_link_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + servobottom_link_8: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + servobottom_link_9: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shell_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 0.5 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 0.34426525235176086 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.0016349668148905039 + Y: -0.014781012199819088 + Z: 0.017814036458730698 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.46039697527885437 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.593582272529602 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 882 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000142000002d9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000037000002d9000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002d9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000037000002d9000000a100fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004c000000044fc0100000002fb0000000800540069006d00650100000000000004c00000026b00fffffffb0000000800540069006d0065010000000000000450000000000000000000000267000002d900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1216 + X: 700 + Y: 385 diff --git a/Spot-Arm/spot_arm_description/meshes/base_link.stl b/Spot-Arm/spot_arm_description/meshes/base_link.stl new file mode 100644 index 00000000..57be0796 Binary files /dev/null and b/Spot-Arm/spot_arm_description/meshes/base_link.stl differ diff --git a/Spot-Arm/spot_arm_description/meshes/finger.stl b/Spot-Arm/spot_arm_description/meshes/finger.stl new file mode 100644 index 00000000..143911c8 Binary files /dev/null and b/Spot-Arm/spot_arm_description/meshes/finger.stl differ diff --git a/Spot-Arm/spot_arm_description/meshes/gripper.stl b/Spot-Arm/spot_arm_description/meshes/gripper.stl new file mode 100644 index 00000000..a89902d3 Binary files /dev/null and b/Spot-Arm/spot_arm_description/meshes/gripper.stl differ diff --git a/Spot-Arm/spot_arm_description/meshes/link1.stl b/Spot-Arm/spot_arm_description/meshes/link1.stl new file mode 100644 index 00000000..e881013a Binary files /dev/null and b/Spot-Arm/spot_arm_description/meshes/link1.stl differ diff --git a/Spot-Arm/spot_arm_description/meshes/link2.stl b/Spot-Arm/spot_arm_description/meshes/link2.stl new file mode 100644 index 00000000..5e4adeec Binary files /dev/null and b/Spot-Arm/spot_arm_description/meshes/link2.stl differ diff --git a/Spot-Arm/spot_arm_description/meshes/link3.stl b/Spot-Arm/spot_arm_description/meshes/link3.stl new file mode 100644 index 00000000..302db2ed Binary files /dev/null and b/Spot-Arm/spot_arm_description/meshes/link3.stl differ diff --git a/Spot-Arm/spot_arm_description/meshes/link4.stl b/Spot-Arm/spot_arm_description/meshes/link4.stl new file mode 100644 index 00000000..0f09446a Binary files /dev/null and b/Spot-Arm/spot_arm_description/meshes/link4.stl differ diff --git a/Spot-Arm/spot_arm_description/meshes/wrist.stl b/Spot-Arm/spot_arm_description/meshes/wrist.stl new file mode 100644 index 00000000..b41f621f Binary files /dev/null and b/Spot-Arm/spot_arm_description/meshes/wrist.stl differ diff --git a/Spot-Arm/spot_arm_description/package.xml b/Spot-Arm/spot_arm_description/package.xml new file mode 100644 index 00000000..25d32413 --- /dev/null +++ b/Spot-Arm/spot_arm_description/package.xml @@ -0,0 +1,59 @@ + + + spot_arm_description + 0.0.0 + The spot_arm_description package + + + + + esther + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + + + + + + + diff --git a/Spot-Arm/spot_arm_description/urdf/materials.xacro b/Spot-Arm/spot_arm_description/urdf/materials.xacro new file mode 100644 index 00000000..7630b63f --- /dev/null +++ b/Spot-Arm/spot_arm_description/urdf/materials.xacro @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/Spot-Arm/spot_arm_description/urdf/spot_arm.gazebo b/Spot-Arm/spot_arm_description/urdf/spot_arm.gazebo new file mode 100644 index 00000000..3473c61f --- /dev/null +++ b/Spot-Arm/spot_arm_description/urdf/spot_arm.gazebo @@ -0,0 +1,66 @@ + + + + + + + + + + ${body_color} + 0.2 + 0.2 + true + true + + + + ${body_color} + 0.2 + 0.2 + true + + + + ${body_color} + 0.2 + 0.2 + true + + + + ${body_color} + 0.2 + 0.2 + true + + + + ${body_color} + 0.2 + 0.2 + true + + + + ${body_color} + 0.2 + 0.2 + true + + + + ${body_color} + 0.2 + 0.2 + true + + + + ${body_color} + 0.2 + 0.2 + true + + + diff --git a/Spot-Arm/spot_arm_description/urdf/spot_arm.trans b/Spot-Arm/spot_arm_description/urdf/spot_arm.trans new file mode 100644 index 00000000..bc17bca4 --- /dev/null +++ b/Spot-Arm/spot_arm_description/urdf/spot_arm.trans @@ -0,0 +1,81 @@ + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + diff --git a/Spot-Arm/spot_arm_description/urdf/spot_arm.xacro b/Spot-Arm/spot_arm_description/urdf/spot_arm.xacro new file mode 100644 index 00000000..7289cc42 --- /dev/null +++ b/Spot-Arm/spot_arm_description/urdf/spot_arm.xacro @@ -0,0 +1,231 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/CMakeLists.txt b/Spot-Arm/spot_moveit_config/CMakeLists.txt new file mode 100644 index 00000000..33f65860 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.1.3) +project(spot_moveit_config) + +find_package(catkin REQUIRED) + +catkin_package() + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/Spot-Arm/spot_moveit_config/config/cartesian_limits.yaml b/Spot-Arm/spot_moveit_config/config/cartesian_limits.yaml new file mode 100644 index 00000000..7df72f69 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/config/cartesian_limits.yaml @@ -0,0 +1,5 @@ +cartesian_limits: + max_trans_vel: 1 + max_trans_acc: 2.25 + max_trans_dec: -5 + max_rot_vel: 1.57 diff --git a/Spot-Arm/spot_moveit_config/config/chomp_planning.yaml b/Spot-Arm/spot_moveit_config/config/chomp_planning.yaml new file mode 100644 index 00000000..75258e53 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.01 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearence: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: true +max_recovery_attempts: 5 \ No newline at end of file diff --git a/Spot-Arm/spot_moveit_config/config/fake_controllers.yaml b/Spot-Arm/spot_moveit_config/config/fake_controllers.yaml new file mode 100644 index 00000000..a61a0fda --- /dev/null +++ b/Spot-Arm/spot_moveit_config/config/fake_controllers.yaml @@ -0,0 +1,20 @@ +controller_list: + - name: fake_spot_arm_controller + type: $(arg execution_type) + joints: + - Joint1 + - Joint2 + - Joint3 + - Joint4 + - Joint5 + - Joint6 + - name: fake_hand_controller + type: $(arg execution_type) + joints: + - Joint6 + - Joint7 +initial: # Define initial robot poses. + - group: spot_arm + pose: Zero + - group: hand + pose: close diff --git a/Spot-Arm/spot_moveit_config/config/joint_limits.yaml b/Spot-Arm/spot_moveit_config/config/joint_limits.yaml new file mode 100644 index 00000000..db026405 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/config/joint_limits.yaml @@ -0,0 +1,39 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + Joint5: + has_velocity_limits: true + max_velocity: 100 + has_acceleration_limits: false + max_acceleration: 0 + Joint3: + has_velocity_limits: true + max_velocity: 100 + has_acceleration_limits: false + max_acceleration: 0 + Joint4: + has_velocity_limits: true + max_velocity: 100 + has_acceleration_limits: false + max_acceleration: 0 + Joint6: + has_velocity_limits: true + max_velocity: 100 + has_acceleration_limits: false + max_acceleration: 0 + Joint7: + has_velocity_limits: true + max_velocity: 100 + has_acceleration_limits: false + max_acceleration: 0 + Joint2: + has_velocity_limits: true + max_velocity: 100 + has_acceleration_limits: false + max_acceleration: 0 + Joint1: + has_velocity_limits: true + max_velocity: 100 + has_acceleration_limits: false + max_acceleration: 0 diff --git a/Spot-Arm/spot_moveit_config/config/kinematics.yaml b/Spot-Arm/spot_moveit_config/config/kinematics.yaml new file mode 100644 index 00000000..f1fb33ca --- /dev/null +++ b/Spot-Arm/spot_moveit_config/config/kinematics.yaml @@ -0,0 +1,4 @@ +spot_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 \ No newline at end of file diff --git a/Spot-Arm/spot_moveit_config/config/ompl_planning.yaml b/Spot-Arm/spot_moveit_config/config/ompl_planning.yaml new file mode 100644 index 00000000..d1600e6c --- /dev/null +++ b/Spot-Arm/spot_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,176 @@ +planner_configs: + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 +spot_arm: + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + projection_evaluator: joints(Rev9,Rev8) + longest_valid_segment_fraction: 0.005 +hand: + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + projection_evaluator: joints(Rev6,Rev7) + longest_valid_segment_fraction: 0.005 diff --git a/Spot-Arm/spot_moveit_config/config/ros_controllers.yaml b/Spot-Arm/spot_moveit_config/config/ros_controllers.yaml new file mode 100644 index 00000000..aaa28afe --- /dev/null +++ b/Spot-Arm/spot_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,66 @@ +# Simulation settings for using moveit_sim_controllers +moveit_sim_hw_interface: + joint_model_group: spot_arm + joint_model_group_pose: Zero +# Settings for ros_control_boilerplate control loop +generic_hw_control_loop: + loop_hz: 300 + cycle_time_error_threshold: 0.01 +# Settings for ros_control hardware interface +hardware_interface: + joints: + - Joint1 + - Joint2 + - Joint3 + - Joint4 + - Joint5 + - Joint6 + - Joint7 + sim_control_mode: 1 # 0: position, 1: velocity +# Publish all joint states +# Creates the /joint_states topic necessary in ROS +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: + [] +arm_position_controller: + type: position_controllers/JointPositionController + joints: + - Joint1 + - Joint2 + - Joint3 + - Joint4 + - Joint5 + - Joint6 + gains: + Joint1: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + Joint2: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + Joint3: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + Joint4: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + Joint5: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + Joint6: + p: 100 + d: 1 + i: 1 + i_clamp: 1 diff --git a/Spot-Arm/spot_moveit_config/config/sensors_3d.yaml b/Spot-Arm/spot_moveit_config/config/sensors_3d.yaml new file mode 100644 index 00000000..d2955dcd --- /dev/null +++ b/Spot-Arm/spot_moveit_config/config/sensors_3d.yaml @@ -0,0 +1,3 @@ +# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it +sensors: + - {} \ No newline at end of file diff --git a/Spot-Arm/spot_moveit_config/config/spot_arm.srdf b/Spot-Arm/spot_moveit_config/config/spot_arm.srdf new file mode 100644 index 00000000..4af6d53c --- /dev/null +++ b/Spot-Arm/spot_moveit_config/config/spot_arm.srdf @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/chomp_planning_pipeline.launch.xml b/Spot-Arm/spot_moveit_config/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 00000000..f079c19f --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/default_warehouse_db.launch b/Spot-Arm/spot_moveit_config/launch/default_warehouse_db.launch new file mode 100644 index 00000000..1ed182b1 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/demo.launch b/Spot-Arm/spot_moveit_config/launch/demo.launch new file mode 100644 index 00000000..cadc55c3 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/demo.launch @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/demo_gazebo.launch b/Spot-Arm/spot_moveit_config/launch/demo_gazebo.launch new file mode 100644 index 00000000..74eb888d --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/demo_gazebo.launch @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + [/joint_states] + + + [move_group/fake_controller_joint_states] + [/joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/Spot-Arm/spot_moveit_config/launch/fake_moveit_controller_manager.launch.xml new file mode 100644 index 00000000..6c9054ee --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/gazebo.launch b/Spot-Arm/spot_moveit_config/launch/gazebo.launch new file mode 100644 index 00000000..605306d6 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/gazebo.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/joystick_control.launch b/Spot-Arm/spot_moveit_config/launch/joystick_control.launch new file mode 100644 index 00000000..9411f6e6 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/move_group.launch b/Spot-Arm/spot_moveit_config/launch/move_group.launch new file mode 100644 index 00000000..ba2418b5 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/move_group.launch @@ -0,0 +1,84 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/moveit.rviz b/Spot-Arm/spot_moveit_config/launch/moveit.rviz new file mode 100644 index 00000000..b431b2a0 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/moveit.rviz @@ -0,0 +1,689 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + Splitter Ratio: 0.74256 + Tree Height: 664 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Time: 5 + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + Name: MotionPlanning + Planned Path: + Links: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trajectory Topic: move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: left_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.2 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Scene Robot: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 2.9965 + Focal Point: + X: 0.113567 + Y: 0.10592 + Z: 2.23518e-07 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.509797 + Target Frame: /base_link + Value: XYOrbit (rviz) + Yaw: 5.65995 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1337 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + Motion Planning: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1828 + X: 459 + Y: -243 diff --git a/Spot-Arm/spot_moveit_config/launch/moveit_rviz.launch b/Spot-Arm/spot_moveit_config/launch/moveit_rviz.launch new file mode 100644 index 00000000..a4605c03 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/ompl_planning_pipeline.launch.xml b/Spot-Arm/spot_moveit_config/launch/ompl_planning_pipeline.launch.xml new file mode 100644 index 00000000..92e75873 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/Spot-Arm/spot_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml new file mode 100644 index 00000000..ac6272e2 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/planning_context.launch b/Spot-Arm/spot_moveit_config/launch/planning_context.launch new file mode 100644 index 00000000..0908dc1b --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/planning_context.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/planning_pipeline.launch.xml b/Spot-Arm/spot_moveit_config/launch/planning_pipeline.launch.xml new file mode 100644 index 00000000..591d82e6 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/planning_pipeline.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/ros_controllers.launch b/Spot-Arm/spot_moveit_config/launch/ros_controllers.launch new file mode 100644 index 00000000..4f27f9c1 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/run_benchmark_ompl.launch b/Spot-Arm/spot_moveit_config/launch/run_benchmark_ompl.launch new file mode 100644 index 00000000..e2ae6110 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/run_benchmark_ompl.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/sensor_manager.launch.xml b/Spot-Arm/spot_moveit_config/launch/sensor_manager.launch.xml new file mode 100644 index 00000000..cc119d37 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/sensor_manager.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/setup_assistant.launch b/Spot-Arm/spot_moveit_config/launch/setup_assistant.launch new file mode 100644 index 00000000..8c7dc62d --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/setup_assistant.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/spot_arm_moveit_controller_manager.launch.xml b/Spot-Arm/spot_moveit_config/launch/spot_arm_moveit_controller_manager.launch.xml new file mode 100644 index 00000000..f1b9d654 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/spot_arm_moveit_controller_manager.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/spot_arm_moveit_sensor_manager.launch.xml b/Spot-Arm/spot_moveit_config/launch/spot_arm_moveit_sensor_manager.launch.xml new file mode 100644 index 00000000..5d02698d --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/spot_arm_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/Spot-Arm/spot_moveit_config/launch/trajectory_execution.launch.xml b/Spot-Arm/spot_moveit_config/launch/trajectory_execution.launch.xml new file mode 100644 index 00000000..26d85f1e --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/trajectory_execution.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/warehouse.launch b/Spot-Arm/spot_moveit_config/launch/warehouse.launch new file mode 100644 index 00000000..15bf7bf4 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/launch/warehouse_settings.launch.xml b/Spot-Arm/spot_moveit_config/launch/warehouse_settings.launch.xml new file mode 100644 index 00000000..e473b083 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Spot-Arm/spot_moveit_config/package.xml b/Spot-Arm/spot_moveit_config/package.xml new file mode 100644 index 00000000..c29be1a7 --- /dev/null +++ b/Spot-Arm/spot_moveit_config/package.xml @@ -0,0 +1,37 @@ + + + spot_moveit_config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the spot_arm with the MoveIt! Motion Planning Framework + + esther + esther + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + catkin + + moveit_ros_move_group + moveit_fake_controller_manager + moveit_kinematics + moveit_planners_ompl + moveit_ros_visualization + moveit_setup_assistant + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + robot_state_publisher + rviz + tf2_ros + xacro + + + spot_arm_description + + + diff --git a/my_grasping/CMakeLists.txt b/my_grasping/CMakeLists.txt new file mode 100644 index 00000000..be24a03e --- /dev/null +++ b/my_grasping/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.0.2) +project(my_grasping) + + +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + roscpp + rospy + std_msgs + message_generation +) + +## Generate messages in the 'msg' folder +add_message_files( + FILES + PixelPose.msg +) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + geometry_msgs + std_msgs +) + +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES my_grasping +# CATKIN_DEPENDS cv2 cv_bridge geometry_msgs roscpp rospy std_msgs +# DEPENDS system_lib +) + + +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + diff --git a/my_grasping/msg/PixelPose.msg b/my_grasping/msg/PixelPose.msg new file mode 100644 index 00000000..84673c62 --- /dev/null +++ b/my_grasping/msg/PixelPose.msg @@ -0,0 +1 @@ +int16[] pixel_pose diff --git a/my_grasping/package.xml b/my_grasping/package.xml new file mode 100644 index 00000000..805029a7 --- /dev/null +++ b/my_grasping/package.xml @@ -0,0 +1,48 @@ + + + my_grasping + 0.0.0 + The my_grasping package + + + + + administrator + + + + + + TODO + + + catkin + cv2 + cv_bridge + geometry_msgs + roscpp + rospy + std_msgs + cv2 + cv_bridge + geometry_msgs + roscpp + rospy + std_msgs + cv2 + cv_bridge + geometry_msgs + roscpp + rospy + std_msgs + + message_generation + message_runtime + + + + + + + + diff --git a/my_grasping/src/image_box.py b/my_grasping/src/image_box.py new file mode 100644 index 00000000..241c3000 --- /dev/null +++ b/my_grasping/src/image_box.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python2.7 + +"""Get an image and command Spot to walk up to the selected object in the image.""" + +import rospy +import roslib +import time, sys +from sensor_msgs.msg import Image +from my_grasping.msg import PixelPose +import cv2 +from cv_bridge import CvBridge, CvBridgeError +from std_srvs.srv import Empty + + +class image_display(): + + def __init__(self): + self.bridge = CvBridge() + + self.g_image_click = None + self.g_image_display = None + self.mU = True + + # Subscriber, Take only 1 image + self.image_subscriber = rospy.Subscriber("/spot/camera/frontright/image", Image, self.image_callback, queue_size=10) + + + def image_callback(self, img_msg): + rospy.loginfo('Image received') + try: + self.cv_image = self.bridge.imgmsg_to_cv2(img_msg, "passthrough") + except CvBridgeError as e: + rospy.logerr("CvBridge Error: {0}".format(e)) + + # Flip image by 90 degrees + self.cv_image = cv2.transpose(self.cv_image) + self.cv_image = cv2.flip(self.cv_image, 1) + + # Show image to user and wait for them to click on a pixel + rospy.loginfo('Click on an object to walk up to') + image_title = 'Click to walk up to something' + cv2.namedWindow(image_title) + cv2.setMouseCallback(image_title, self.cv_mouse_callback) + + self.g_image_display = self.cv_image + cv2.imshow(image_title, self.g_image_display) + + while self.g_image_click is None: + key = cv2.waitKey(1) & 0xFF + if key == ord('q') or key == ord('Q'): + # Quit + print('"q" pressed, exiting.') + exit(0) + + rospy.loginfo('Walking to object at image location: ' + str(self.g_image_click[0]) + ' ' + str(self.g_image_click[1])) + + my_click = [] + my_click.append(self.g_image_click[0]) + my_click.append(self.g_image_click[1]) + + # Publishing location of the object in the image + while self.mU == True: + print(my_click) + self.location_publisher.publish(my_click) + self.mU = False + + + def cv_mouse_callback(self, event, x, y, flags, param): + clone = self.g_image_display.copy() + if event == cv2.EVENT_LBUTTONUP: + self.g_image_click = (x, y) + else: + # Draw some lines on the image. + # print('mouse', x, y) + color = (30, 30, 30) + thickness = 2 + image_title = 'Click to walk up to something' + height = clone.shape[0] + width = clone.shape[1] + cv2.line(clone, (0, y), (width, y), color, thickness) + cv2.line(clone, (x, 0), (x, height), color, thickness) + cv2.imshow(image_title, clone) + + + def main(self): + rospy.init_node('image_display', anonymous=True) + self.location_publisher = rospy.Publisher('object_location', PixelPose, queue_size=10) + + try: + rospy.spin() + except KeyboardInterrupt: + print "Shutting down ROS Image feature detector module" + cv2.destroyAllWindows() + +if __name__ == "__main__": + imdis = image_display() + imdis.main() diff --git a/spot_arm.mp4 b/spot_arm.mp4 new file mode 100644 index 00000000..6609f38f Binary files /dev/null and b/spot_arm.mp4 differ diff --git a/spot_description/launch/description.launch b/spot_description/launch/description.launch index 5789d377..dd9893b5 100644 --- a/spot_description/launch/description.launch +++ b/spot_description/launch/description.launch @@ -1,5 +1,5 @@ - + diff --git a/spot_description/urdf/spot.urdf.xacro b/spot_description/urdf/spot.urdf.xacro index 48e5cd36..a7339451 100644 --- a/spot_description/urdf/spot.urdf.xacro +++ b/spot_description/urdf/spot.urdf.xacro @@ -12,13 +12,18 @@ - - + + + + + + + diff --git a/spot_driver/config/spot_ros.yaml b/spot_driver/config/spot_ros.yaml index 6be5b30d..53831c33 100644 --- a/spot_driver/config/spot_ros.yaml +++ b/spot_driver/config/spot_ros.yaml @@ -1,10 +1,12 @@ rates: robot_state: 20.0 + localization_state: 20.0 metrics: 0.04 lease: 1.0 front_image: 10.0 side_image: 10.0 rear_image: 10.0 + hand_image: 10.0 auto_claim: False auto_power_on: False auto_stand: False diff --git a/spot_driver/launch/driver.launch b/spot_driver/launch/driver.launch index e19b091f..ec8e3bcf 100644 --- a/spot_driver/launch/driver.launch +++ b/spot_driver/launch/driver.launch @@ -1,7 +1,7 @@ - - - + + + diff --git a/spot_driver/src/spot_driver/graph_nav_util.py b/spot_driver/src/spot_driver/graph_nav_util.py index 96dd5c11..cdd8b9d8 100644 --- a/spot_driver/src/spot_driver/graph_nav_util.py +++ b/spot_driver/src/spot_driver/graph_nav_util.py @@ -105,7 +105,7 @@ def update_waypoints_and_edges(graph, localization_id, logger): edges[edge.id.to_waypoint].append(edge.id.from_waypoint) else: edges[edge.id.to_waypoint] = [edge.id.from_waypoint] - logger.info("(Edge) from waypoint id: ", edge.id.from_waypoint, " and to waypoint id: ", + logger.info("(Edge) from waypoint id: " + edge.id.from_waypoint + " and to waypoint id: " + edge.id.to_waypoint) return name_to_id, edges diff --git a/spot_driver/src/spot_driver/ros_helpers.py b/spot_driver/src/spot_driver/ros_helpers.py index 1a88f599..9574b229 100644 --- a/spot_driver/src/spot_driver/ros_helpers.py +++ b/spot_driver/src/spot_driver/ros_helpers.py @@ -5,7 +5,7 @@ from geometry_msgs.msg import TransformStamped from sensor_msgs.msg import Image, CameraInfo from sensor_msgs.msg import JointState -from geometry_msgs.msg import PoseWithCovariance +from geometry_msgs.msg import Pose, PoseWithCovariance from geometry_msgs.msg import TwistWithCovariance from geometry_msgs.msg import TwistWithCovarianceStamped from nav_msgs.msg import Odometry @@ -16,13 +16,15 @@ from spot_msgs.msg import EStopState, EStopStateArray from spot_msgs.msg import WiFiState from spot_msgs.msg import PowerState +from spot_msgs.msg import ManipulatorState from spot_msgs.msg import BehaviorFault, BehaviorFaultState from spot_msgs.msg import SystemFault, SystemFaultState from spot_msgs.msg import BatteryState, BatteryStateArray - +from spot_msgs.msg import LocalizationState from bosdyn.api import image_pb2 -from bosdyn.client.math_helpers import SE3Pose +from bosdyn.client.math_helpers import SE3Pose, SE3Velocity, Vec3 from bosdyn.client.frame_helpers import get_odom_tform_body, get_vision_tform_body +from std_msgs.msg import Bool friendly_joint_names = {} """Dictionary for mapping BD joint names to more friendly names""" @@ -39,6 +41,16 @@ friendly_joint_names["hr.hy"] = "rear_right_hip_y" friendly_joint_names["hr.kn"] = "rear_right_knee" +# Arm joint names +friendly_joint_names["arm0.sh0"] = "Joint1" +friendly_joint_names["arm0.sh1"] = "Joint2" +friendly_joint_names["arm0.el0"] = "Joint3" +friendly_joint_names["arm0.el1"] = "Joint4" +friendly_joint_names["arm0.wr0"] = "Joint5" +friendly_joint_names["arm0.wr1"] = "Joint6" +friendly_joint_names["arm0.f1x"] = "Joint7" + + class DefaultCameraInfo(CameraInfo): """Blank class extending CameraInfo ROS topic that defaults most parameters""" def __init__(self): @@ -190,6 +202,7 @@ def GetJointStatesFromState(state, spot_wrapper): joint_state = JointState() local_time = spot_wrapper.robotToLocalTime(state.kinematic_state.acquisition_timestamp) joint_state.header.stamp = rospy.Time(local_time.seconds, local_time.nanos) + for joint in state.kinematic_state.joint_states: joint_state.name.append(friendly_joint_names.get(joint.name, "ERROR")) joint_state.position.append(joint.position.value) @@ -293,6 +306,18 @@ def GetOdomFromState(state, spot_wrapper, use_vision=True): odom_msg.twist = twist_odom_msg return odom_msg +def GetLocalizationFromState(state, spot_wrapper): + localization_msg = LocalizationState() + if not state.localization.waypoint_id: + localization_msg.is_localized = False + return localization_msg + localization_msg.waypoint_id = state.localization.waypoint_id + localization_msg.waypoint_tform_body.pose = toROSPose(state.localization.waypoint_tform_body) + localization_msg.seed_tform_body.pose = toROSPose(state.localization.waypoint_tform_body) + local_time = spot_wrapper.robotToLocalTime(state.localization.timestamp) + localization_msg.timestamp = rospy.Time(local_time.seconds, local_time.nanos) + return localization_msg + def GetWifiFromState(state, spot_wrapper): """Maps wireless state data from robot state proto to ROS WiFiState message @@ -323,6 +348,7 @@ def GetTFFromState(state, spot_wrapper, inverse_target_frame): tf_msg = TFMessage() for frame_name in state.kinematic_state.transforms_snapshot.child_to_parent_edge_map: + # rospy.loginfo(frame_name) if state.kinematic_state.transforms_snapshot.child_to_parent_edge_map.get(frame_name).parent_frame_name: try: transform = state.kinematic_state.transforms_snapshot.child_to_parent_edge_map.get(frame_name) @@ -370,7 +396,7 @@ def GetPowerStatesFromState(state, spot_wrapper): """Maps power state data from robot state proto to ROS PowerState message Args: - data: Robot State proto + state: Robot State proto spot_wrapper: A SpotWrapper object Returns: PowerState message @@ -384,6 +410,16 @@ def GetPowerStatesFromState(state, spot_wrapper): power_state_msg.locomotion_estimated_runtime = rospy.Time(state.power_state.locomotion_estimated_runtime.seconds, state.power_state.locomotion_estimated_runtime.nanos) return power_state_msg +def GetManipulatorStateFromState(state, spot_wrapper): + msg = ManipulatorState() + msg.gripper_open_percentage = state.manipulator_state.gripper_open_percentage + msg.is_gripper_holding_item = state.manipulator_state.is_gripper_holding_item + msg.estimated_end_effector_force_in_hand = Vec3.from_proto(state.manipulator_state.estimated_end_effector_force_in_hand) + msg.stow_state = state.manipulator_state.stow_state + msg.velocity_of_hand_in_vision = SE3Velocity.from_proto(state.manipulator_state.velocity_of_hand_in_vision) + msg.velocity_of_hand_in_odom = SE3Velocity.from_proto(state.manipulator_state.velocity_of_hand_in_odom) + return msg + def getBehaviorFaults(behavior_faults, spot_wrapper): """Helper function to strip out behavior faults into a list @@ -461,3 +497,14 @@ def getBehaviorFaultsFromState(state, spot_wrapper): behavior_fault_state_msg = BehaviorFaultState() behavior_fault_state_msg.faults = getBehaviorFaults(state.behavior_fault_state.faults, spot_wrapper) return behavior_fault_state_msg + +def toROSPose(state): + p = Pose() + p.position.x = state.position.x + p.position.y = state.position.y + p.position.z = state.position.z + p.orientation.x = state.rotation.x + p.orientation.y = state.rotation.y + p.orientation.z = state.rotation.z + p.orientation.w = state.rotation.w + return p \ No newline at end of file diff --git a/spot_driver/src/spot_driver/spot_ros.py b/spot_driver/src/spot_driver/spot_ros.py index bbfb9206..bb2f5b4b 100644 --- a/spot_driver/src/spot_driver/spot_ros.py +++ b/spot_driver/src/spot_driver/spot_ros.py @@ -1,4 +1,5 @@ import rospy +from my_grasping.msg import PixelPose from std_srvs.srv import Trigger, TriggerResponse, SetBool, SetBoolResponse from std_msgs.msg import Bool @@ -6,7 +7,7 @@ from geometry_msgs.msg import TransformStamped from sensor_msgs.msg import Image, CameraInfo from sensor_msgs.msg import JointState -from geometry_msgs.msg import TwistWithCovarianceStamped, Twist, Pose +from geometry_msgs.msg import TwistWithCovarianceStamped, Twist, PoseStamped, Pose from nav_msgs.msg import Odometry from bosdyn.api.spot import robot_command_pb2 as spot_command_pb2 @@ -31,11 +32,19 @@ from spot_msgs.msg import MobilityParams from spot_msgs.msg import NavigateToAction, NavigateToResult, NavigateToFeedback from spot_msgs.msg import TrajectoryAction, TrajectoryResult, TrajectoryFeedback +from spot_msgs.msg import GripperAngleAction, GripperAngleResult from spot_msgs.srv import ListGraph, ListGraphResponse from spot_msgs.srv import SetLocomotion, SetLocomotionResponse from spot_msgs.srv import ClearBehaviorFault, ClearBehaviorFaultResponse from spot_msgs.srv import SetVelocity, SetVelocityResponse - +from spot_msgs.srv import ListTaggedObjects, ListTaggedObjectsResponse +from spot_msgs.srv import GetObjectPose, GetObjectPoseResponse +from spot_msgs.srv import ArmJointMovement, ArmJointMovementResponse, ArmJointMovementRequest +from spot_msgs.srv import GripperAngleMove, GripperAngleMoveResponse, GripperAngleMoveRequest +from spot_msgs.srv import ArmForceTrajectory, ArmForceTrajectoryResponse, ArmForceTrajectoryRequest +from spot_msgs.srv import HandPose, HandPoseResponse, HandPoseRequest +from spot_msgs.srv import LocalizeInGraph, LocalizeInGraphResponse +from spot_msgs.srv import DockRobot, DockRobotResponse from .ros_helpers import * from .spot_wrapper import SpotWrapper @@ -52,11 +61,13 @@ def __init__(self): self.callbacks = {} """Dictionary listing what callback to use for what data task""" self.callbacks["robot_state"] = self.RobotStateCB + self.callbacks["localization_state"] = self.LocalizationStateCB self.callbacks["metrics"] = self.MetricsCB self.callbacks["lease"] = self.LeaseCB self.callbacks["front_image"] = self.FrontImageCB self.callbacks["side_image"] = self.SideImageCB self.callbacks["rear_image"] = self.RearImageCB + self.callbacks["hand_image"] = self.HandImageCB def RobotStateCB(self, results): """Callback for when the Spot Wrapper gets new robot state data. @@ -107,6 +118,10 @@ def RobotStateCB(self, results): power_state_msg = GetPowerStatesFromState(state, self.spot_wrapper) self.power_pub.publish(power_state_msg) + # Manipulator State # + manipulator_state_msg = GetManipulatorStateFromState(state, self.spot_wrapper) + self.manipulator_pub.publish(manipulator_state_msg) + # System Faults # system_fault_state_msg = GetSystemFaultsFromState(state, self.spot_wrapper) self.system_faults_pub.publish(system_fault_state_msg) @@ -115,6 +130,13 @@ def RobotStateCB(self, results): behavior_fault_state_msg = getBehaviorFaultsFromState(state, self.spot_wrapper) self.behavior_faults_pub.publish(behavior_fault_state_msg) + def LocalizationStateCB(self, results): + localization_state = self.spot_wrapper.localization_state + if localization_state: + localization_msg = GetLocalizationFromState(localization_state, self.spot_wrapper) + self.localization_pub.publish(localization_msg) + + def MetricsCB(self, results): """Callback for when the Spot Wrapper gets new metrics data. @@ -233,6 +255,32 @@ def RearImageCB(self, results): self.populate_camera_static_transforms(data[0]) self.populate_camera_static_transforms(data[1]) + + def HandImageCB(self, results): + """Callback for when the Spot Wrapper gets new hand image data. + + Args: + results: FutureWrapper object of AsyncPeriodicQuery callback + """ + data = self.spot_wrapper.hand_images + if data: + mage_msg0, camera_info_msg0 = getImageMsg(data[0], self.spot_wrapper) + self.hand_image_mono_pub.publish(mage_msg0) + self.hand_image_mono_info_pub.publish(camera_info_msg0) + mage_msg1, camera_info_msg1 = getImageMsg(data[1], self.spot_wrapper) + self.hand_depth_pub.publish(mage_msg1) + self.hand_depth_info_pub.publish(camera_info_msg1) + image_msg2, camera_info_msg2 = getImageMsg(data[2], self.spot_wrapper) + self.hand_image_color_pub.publish(image_msg2) + self.hand_image_color_info_pub.publish(camera_info_msg2) + image_msg3, camera_info_msg3 = getImageMsg(data[3], self.spot_wrapper) + self.hand_depth_in_hand_color_pub.publish(image_msg3) + self.hand_depth_in_color_info_pub.publish(camera_info_msg3) + + self.populate_camera_static_transforms(data[0]) + self.populate_camera_static_transforms(data[1]) + self.populate_camera_static_transforms(data[2]) + self.populate_camera_static_transforms(data[3]) def handle_claim(self, req): """ROS service handler for the claim service""" @@ -264,6 +312,13 @@ def handle_stand(self, req): resp = self.spot_wrapper.stand() return TriggerResponse(resp[0], resp[1]) + def handle_dock(self, req): + resp = self.spot_wrapper.dock(dock_id=int(req.id)) + return DockRobotResponse(resp[0], resp[1]) + + def handle_undock(self, req): + resp = self.spot_wrapper.undock() + return TriggerResponse(resp[0], resp[1]) def handle_power_on(self, req): """ROS service handler for the power-on service""" resp = self.spot_wrapper.power_on() @@ -336,6 +391,37 @@ def handle_max_vel(self, req): except Exception as e: return SetVelocityResponse(False, 'Error:{}'.format(e)) + def handle_list_tagged_objects(self, req): + object_ids = self.spot_wrapper.list_tagged_objects() + resp = ListTaggedObjectsResponse() + resp.waypoint_ids = object_ids + return resp + + def handle_localize_in_graph(self, req): + success, message = self.spot_wrapper.localize_in_graph(req.upload_path, initial_localization_fiducial=req.initial_localization_fiducial, + initial_localization_waypoint=req.initial_localization_waypoint) + return LocalizeInGraphResponse(success=success, message=message) + + def handle_get_tagged_object_pose(self, req): + resp = self.spot_wrapper.get_object_pose(req.id) + if (resp[0]): + # convert SE3 Pose to pose (move to helper function plz future jacob) + p = PoseStamped() + p.header.frame_id = "odom" + print(resp[1]) + p.pose.position.x = resp[1].position.x + p.pose.position.y = resp[1].position.y + p.pose.position.z = resp[1].position.z + p.pose.orientation.x = resp[1].rotation.x + p.pose.orientation.y = resp[1].rotation.y + p.pose.orientation.z = resp[1].rotation.z + p.pose.orientation.w = resp[1].rotation.w + return GetObjectPoseResponse(resp[0], "success!", p) + r = GetObjectPoseResponse() + r.success = False + r.message = "Unable to find object with fiducial ID " + req.id + return r + def handle_trajectory(self, req): """ROS actionserver execution handler to handle receiving a request to move to a location""" if req.target_pose.header.frame_id != 'body': @@ -396,6 +482,14 @@ def timeout_cb(trajectory_server, _): self.trajectory_server.publish_feedback(TrajectoryFeedback("Failed to reach goal")) self.trajectory_server.set_aborted(TrajectoryResult(False, "Failed to reach goal")) + def handle_gripper_move(self, req): + resp = self.spot_wrapper.gripper_angle_open(req.gripper_angle) + if resp[0]: + self.gripper_angle_open_as.set_succeeded(GripperAngleResult(resp[0])) + else: + self.gripper_angle_open_as.set_aborted(GripperAngleResult(resp[0])) + + def cmdVelCallback(self, data): """Callback for cmd_vel command""" self.spot_wrapper.velocity_cmd(data.linear.x, data.linear.y, data.angular.z) @@ -426,10 +520,14 @@ def handle_navigate_to_feedback(self): """Thread function to send navigate_to feedback""" while not rospy.is_shutdown() and self.run_navigate_to: localization_state = self.spot_wrapper._graph_nav_client.get_localization_state() + feedback = self.spot_wrapper._graph_nav_client.navigation_feedback() + if localization_state.localization.waypoint_id: - self.navigate_as.publish_feedback(NavigateToFeedback(localization_state.localization.waypoint_id)) + self.navigate_as.publish_feedback(NavigateToFeedback(waypoint_id=localization_state.localization.waypoint_id, + status=feedback.status)) rospy.Rate(10).sleep() + def handle_navigate_to(self, msg): """ROS service handler to run mission of the robot. The robot will replay a mission""" # create thread to periodically publish feedback @@ -480,6 +578,68 @@ def populate_camera_static_transforms(self, image_data): self.camera_static_transforms.append(static_tf) self.camera_static_transform_broadcaster.sendTransform(self.camera_static_transforms) + # Arm functions ################################################## + def handle_arm_stow(self, srv_data): + """ROS service handler to command the arm to stow, home position """ + resp = self.spot_wrapper.arm_stow() + return TriggerResponse(resp[0], resp[1]) + + def handle_arm_unstow(self, srv_data): + """ROS service handler to command the arm to unstow, joints are all zeros""" + resp = self.spot_wrapper.arm_unstow() + return TriggerResponse(resp[0], resp[1]) + + def handle_arm_joint_move(self, srv_data: ArmJointMovementRequest): + """ROS service handler to send joint movement to the arm to execute""" + resp = self.spot_wrapper.arm_joint_move(joint_targets = srv_data.joint_target) + return ArmJointMovementResponse(resp[0], resp[1]) + + def handle_force_trajectory(self, srv_data: ArmForceTrajectoryRequest): + """ROS service handler to send a force trajectory up or down a vertical force""" + resp = self.spot_wrapper.force_trajectory(forces_torques = srv_data.force_torque) + return ArmForceTrajectoryResponse(resp[0], resp[1]) + + def handle_gripper_open(self, srv_data): + """ROS service handler to open the gripper""" + resp = self.spot_wrapper.gripper_open() + return TriggerResponse(resp[0], resp[1]) + + def handle_gripper_close(self, srv_data): + """ROS service handler to close the gripper""" + resp = self.spot_wrapper.gripper_close() + return TriggerResponse(resp[0], resp[1]) + + def handle_gripper_angle_open(self, srv_data: GripperAngleMoveRequest): + """ROS service handler to open the gripper at an angle""" + resp = self.spot_wrapper.gripper_angle_open(gripper_ang = srv_data.gripper_angle) + return GripperAngleMoveResponse(resp[0], resp[1]) + + def handle_arm_carry(self, srv_data): + """ROS service handler to put arm in carry mode""" + resp = self.spot_wrapper.arm_carry() + return TriggerResponse(resp[0], resp[1]) + + def handle_body_follow_arm(self, srv_data): + """ROS service to send a pose to the end effector""" + resp = self.spot_wrapper.hand_position_3d() + return TriggerResponse(resp[0], resp[1]) + + def handle_hand_pose(self, srv_data: HandPoseRequest): + """ROS service to give a position to the gripper""" + resp = self.spot_wrapper.hand_pose(pose_points = srv_data.pose_point, wrist_tform_tool=srv_data.wrist_tform_tool) + return HandPoseResponse(resp[0], resp[1]) + + def walk_to_obj_callback(self, obj_point): + """Callback for pixel points, object to walk""" + rospy.loginfo("Pixel for location to walk to: " + str(obj_point)) + the_click = [] + the_click.append(obj_point.pixel_pose[0]) + the_click.append(obj_point.pixel_pose[1]) + self.spot_wrapper.walk_to_object_image(object_point = the_click) + + + ################################################################## + def shutdown(self): rospy.loginfo("Shutting down ROS driver for Spot") self.spot_wrapper.sit() @@ -529,12 +689,19 @@ def main(self): self.frontright_image_pub = rospy.Publisher('camera/frontright/image', Image, queue_size=10) self.left_image_pub = rospy.Publisher('camera/left/image', Image, queue_size=10) self.right_image_pub = rospy.Publisher('camera/right/image', Image, queue_size=10) + self.hand_image_mono_pub = rospy.Publisher('camera/hand_mono/image', Image, queue_size=10) + self.hand_image_color_pub = rospy.Publisher('camera/hand_color/image', Image, queue_size=10) + # Depth # self.back_depth_pub = rospy.Publisher('depth/back/image', Image, queue_size=10) self.frontleft_depth_pub = rospy.Publisher('depth/frontleft/image', Image, queue_size=10) self.frontright_depth_pub = rospy.Publisher('depth/frontright/image', Image, queue_size=10) self.left_depth_pub = rospy.Publisher('depth/left/image', Image, queue_size=10) self.right_depth_pub = rospy.Publisher('depth/right/image', Image, queue_size=10) + self.hand_depth_pub = rospy.Publisher('depth/hand/image', Image, queue_size=10) + self.hand_depth_in_hand_color_pub = rospy.Publisher('depth/hand/depth_in_color', Image, queue_size=10) + self.frontleft_depth_in_visual_pub = rospy.Publisher('depth/frontleft/depth_in_visual', Image, queue_size=10) + self.frontright_depth_in_visual_pub = rospy.Publisher('depth/frontright/depth_in_visual', Image, queue_size=10) # Image Camera Info # self.back_image_info_pub = rospy.Publisher('camera/back/camera_info', CameraInfo, queue_size=10) @@ -542,12 +709,20 @@ def main(self): self.frontright_image_info_pub = rospy.Publisher('camera/frontright/camera_info', CameraInfo, queue_size=10) self.left_image_info_pub = rospy.Publisher('camera/left/camera_info', CameraInfo, queue_size=10) self.right_image_info_pub = rospy.Publisher('camera/right/camera_info', CameraInfo, queue_size=10) + self.hand_image_mono_info_pub = rospy.Publisher('camera/hand_mono/camera_info', CameraInfo, queue_size=10) + self.hand_image_color_info_pub = rospy.Publisher('camera/hand_color/camera_info', CameraInfo, queue_size=10) + # Depth Camera Info # self.back_depth_info_pub = rospy.Publisher('depth/back/camera_info', CameraInfo, queue_size=10) self.frontleft_depth_info_pub = rospy.Publisher('depth/frontleft/camera_info', CameraInfo, queue_size=10) self.frontright_depth_info_pub = rospy.Publisher('depth/frontright/camera_info', CameraInfo, queue_size=10) self.left_depth_info_pub = rospy.Publisher('depth/left/camera_info', CameraInfo, queue_size=10) self.right_depth_info_pub = rospy.Publisher('depth/right/camera_info', CameraInfo, queue_size=10) + self.hand_depth_info_pub = rospy.Publisher('depth/hand/camera_info', CameraInfo, queue_size=10) + self.hand_depth_in_color_info_pub = rospy.Publisher('camera/hand/depth_in_color/camera_info', CameraInfo, queue_size=10) + self.frontleft_depth_in_visual_info_pub = rospy.Publisher('depth/frontleft/depth_in_visual/camera_info', CameraInfo, queue_size=10) + self.frontright_depth_in_visual_info_pub = rospy.Publisher('depth/frontright/depth_in_visual/camera_info', CameraInfo, queue_size=10) + # Status Publishers # self.joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=10) @@ -564,13 +739,16 @@ def main(self): self.battery_pub = rospy.Publisher('status/battery_states', BatteryStateArray, queue_size=10) self.behavior_faults_pub = rospy.Publisher('status/behavior_faults', BehaviorFaultState, queue_size=10) self.system_faults_pub = rospy.Publisher('status/system_faults', SystemFaultState, queue_size=10) - + self.manipulator_pub = rospy.Publisher('status/manipulator_state', ManipulatorState, queue_size=10) self.feedback_pub = rospy.Publisher('status/feedback', Feedback, queue_size=10) - + self.localization_pub = rospy.Publisher('status/localization_state', LocalizationState, queue_size=10) self.mobility_params_pub = rospy.Publisher('status/mobility_params', MobilityParams, queue_size=10) rospy.Subscriber('cmd_vel', Twist, self.cmdVelCallback, queue_size = 1) rospy.Subscriber('body_pose', Pose, self.bodyPoseCallback, queue_size = 1) + + # Walk to an object + rospy.Subscriber('object_location', PixelPose, self.walk_to_obj_callback, queue_size=1) rospy.Service("claim", Trigger, self.handle_claim) rospy.Service("release", Trigger, self.handle_release) @@ -578,6 +756,8 @@ def main(self): rospy.Service("self_right", Trigger, self.handle_self_right) rospy.Service("sit", Trigger, self.handle_sit) rospy.Service("stand", Trigger, self.handle_stand) + rospy.Service("dock", DockRobot, self.handle_dock) + rospy.Service("undock", Trigger, self.handle_undock) rospy.Service("power_on", Trigger, self.handle_power_on) rospy.Service("power_off", Trigger, self.handle_safe_power_off) @@ -590,13 +770,31 @@ def main(self): rospy.Service("max_velocity", SetVelocity, self.handle_max_vel) rospy.Service("clear_behavior_fault", ClearBehaviorFault, self.handle_clear_behavior_fault) + rospy.Service("list_tagged_objects", ListTaggedObjects, self.handle_list_tagged_objects) + rospy.Service("get_object_pose", GetObjectPose, self.handle_get_tagged_object_pose) + rospy.Service("localize_in_graph", LocalizeInGraph, self.handle_localize_in_graph) rospy.Service("list_graph", ListGraph, self.handle_list_graph) + # Arm Services ######################################### + rospy.Service("arm_stow", Trigger, self.handle_arm_stow) + rospy.Service("arm_unstow", Trigger, self.handle_arm_unstow) + rospy.Service("gripper_open", Trigger, self.handle_gripper_open) + rospy.Service("gripper_close", Trigger, self.handle_gripper_close) + rospy.Service("arm_carry", Trigger, self.handle_arm_carry) + rospy.Service("gripper_angle_open", GripperAngleMove, self.handle_gripper_angle_open) + rospy.Service("arm_joint_move", ArmJointMovement, self.handle_arm_joint_move) + rospy.Service("force_trajectory", ArmForceTrajectory, self.handle_force_trajectory) + rospy.Service("body_follow_hand", Trigger, self.handle_body_follow_arm) + rospy.Service("gripper_pose", HandPose, self.handle_hand_pose) + ######################################################### + + self.navigate_as = actionlib.SimpleActionServer('navigate_to', NavigateToAction, execute_cb = self.handle_navigate_to, auto_start = False) self.navigate_as.start() - + self.gripper_angle_open_as = actionlib.SimpleActionServer('gripper_angle', GripperAngleAction, execute_cb=self.handle_gripper_move, auto_start=False) + self.gripper_angle_open_as.start() self.trajectory_server = actionlib.SimpleActionServer("trajectory", TrajectoryAction, execute_cb=self.handle_trajectory, auto_start=False) diff --git a/spot_driver/src/spot_driver/spot_wrapper.py b/spot_driver/src/spot_driver/spot_wrapper.py index a9f66e77..2f613727 100644 --- a/spot_driver/src/spot_driver/spot_wrapper.py +++ b/spot_driver/src/spot_driver/spot_wrapper.py @@ -1,18 +1,26 @@ import time import math +import os +from typing import List +import rospy from bosdyn.client import create_standard_sdk, ResponseError, RpcError +from bosdyn.client import robot_command from bosdyn.client.async_tasks import AsyncPeriodicQuery, AsyncTasks from bosdyn.geometry import EulerZXY - +from bosdyn.client.docking import blocking_dock_robot, blocking_undock from bosdyn.client.robot_state import RobotStateClient -from bosdyn.client.robot_command import RobotCommandClient, RobotCommandBuilder +from bosdyn.client.robot_command import RobotCommandClient, RobotCommandBuilder, CommandFailedError, blocking_stand from bosdyn.client.graph_nav import GraphNavClient -from bosdyn.client.frame_helpers import get_odom_tform_body +from bosdyn.client.world_object import WorldObjectClient +from bosdyn.client.frame_helpers import get_odom_tform_body, get_a_tform_b, ODOM_FRAME_NAME, \ + GRAV_ALIGNED_BODY_FRAME_NAME, BODY_FRAME_NAME from bosdyn.client.power import safe_power_off, PowerClient, power_on from bosdyn.client.lease import LeaseClient, LeaseKeepAlive from bosdyn.client.image import ImageClient, build_image_request -from bosdyn.api import image_pb2 +from bosdyn.client.manipulation_api_client import ManipulationApiClient +from bosdyn.api import image_pb2, robot_state_pb2, world_object_pb2, manipulation_api_pb2, gripper_command_pb2 +from bosdyn.api.geometry_pb2 import Quaternion from bosdyn.api.graph_nav import graph_nav_pb2 from bosdyn.api.graph_nav import map_pb2 from bosdyn.api.graph_nav import nav_pb2 @@ -24,8 +32,15 @@ from . import graph_nav_util +from bosdyn.api import arm_command_pb2 import bosdyn.api.robot_state_pb2 as robot_state_proto from bosdyn.api import basic_command_pb2 +from bosdyn.api import synchronized_command_pb2 +from bosdyn.api import robot_command_pb2 +from bosdyn.api import geometry_pb2 +from bosdyn.api import trajectory_pb2 +from bosdyn.util import seconds_to_duration +from google.protobuf.duration_pb2 import Duration from google.protobuf.timestamp_pb2 import Timestamp front_image_sources = ['frontleft_fisheye_image', 'frontright_fisheye_image', 'frontleft_depth', 'frontright_depth'] @@ -34,6 +49,9 @@ """List of image sources for side image periodic query""" rear_image_sources = ['back_fisheye_image', 'back_depth'] """List of image sources for rear image periodic query""" +hand_image_sources = ['hand_image', 'hand_depth', 'hand_color_image', 'hand_depth_in_hand_color_frame'] +"""List of image sources for hand image periodic query""" + class AsyncRobotState(AsyncPeriodicQuery): """Class to get robot state at regular intervals. get_robot_state_async query sent to the robot at every tick. Callback registered to defined callback function. @@ -44,9 +62,10 @@ class AsyncRobotState(AsyncPeriodicQuery): rate: Rate (Hz) to trigger the query callback: Callback function to call when the results of the query are available """ + def __init__(self, client, logger, rate, callback): super(AsyncRobotState, self).__init__("robot-state", client, logger, - period_sec=1.0/max(rate, 1.0)) + period_sec=1.0 / max(rate, 1.0)) self._callback = None if rate > 0.0: self._callback = callback @@ -57,6 +76,31 @@ def _start_query(self): callback_future.add_done_callback(self._callback) return callback_future + +class AsyncLocalizationState(AsyncPeriodicQuery): + """Class to get robot state at regular intervals. get_robot_state_async query sent to the robot at every tick. Callback registered to defined callback function. + + Attributes: + client: The Client to a service on the robot + logger: Logger object + rate: Rate (Hz) to trigger the query + callback: Callback function to call when the results of the query are available + """ + + def __init__(self, client, logger, rate, callback): + super(AsyncLocalizationState, self).__init__("localization-state", client, logger, + period_sec=1.0 / max(rate, 1.0)) + self._callback = None + if rate > 0.0: + self._callback = callback + + def _start_query(self): + if self._callback: + callback_future = self._client.get_localization_state_async() + callback_future.add_done_callback(self._callback) + return callback_future + + class AsyncMetrics(AsyncPeriodicQuery): """Class to get robot metrics at regular intervals. get_robot_metrics_async query sent to the robot at every tick. Callback registered to defined callback function. @@ -66,9 +110,10 @@ class AsyncMetrics(AsyncPeriodicQuery): rate: Rate (Hz) to trigger the query callback: Callback function to call when the results of the query are available """ + def __init__(self, client, logger, rate, callback): super(AsyncMetrics, self).__init__("robot-metrics", client, logger, - period_sec=1.0/max(rate, 1.0)) + period_sec=1.0 / max(rate, 1.0)) self._callback = None if rate > 0.0: self._callback = callback @@ -79,6 +124,7 @@ def _start_query(self): callback_future.add_done_callback(self._callback) return callback_future + class AsyncLease(AsyncPeriodicQuery): """Class to get lease state at regular intervals. list_leases_async query sent to the robot at every tick. Callback registered to defined callback function. @@ -88,9 +134,10 @@ class AsyncLease(AsyncPeriodicQuery): rate: Rate (Hz) to trigger the query callback: Callback function to call when the results of the query are available """ + def __init__(self, client, logger, rate, callback): super(AsyncLease, self).__init__("lease", client, logger, - period_sec=1.0/max(rate, 1.0)) + period_sec=1.0 / max(rate, 1.0)) self._callback = None if rate > 0.0: self._callback = callback @@ -101,6 +148,7 @@ def _start_query(self): callback_future.add_done_callback(self._callback) return callback_future + class AsyncImageService(AsyncPeriodicQuery): """Class to get images at regular intervals. get_image_from_sources_async query sent to the robot at every tick. Callback registered to defined callback function. @@ -110,9 +158,10 @@ class AsyncImageService(AsyncPeriodicQuery): rate: Rate (Hz) to trigger the query callback: Callback function to call when the results of the query are available """ + def __init__(self, client, logger, rate, callback, image_requests): super(AsyncImageService, self).__init__("robot_image_service", client, logger, - period_sec=1.0/max(rate, 1.0)) + period_sec=1.0 / max(rate, 1.0)) self._callback = None if rate > 0.0: self._callback = callback @@ -124,6 +173,7 @@ def _start_query(self): callback_future.add_done_callback(self._callback) return callback_future + class AsyncIdle(AsyncPeriodicQuery): """Class to check if the robot is moving, and if not, command a stand with the set mobility parameters @@ -133,9 +183,10 @@ class AsyncIdle(AsyncPeriodicQuery): rate: Rate (Hz) to trigger the query spot_wrapper: A handle to the wrapper library """ + def __init__(self, client, logger, rate, spot_wrapper): super(AsyncIdle, self).__init__("idle", client, logger, - period_sec=1.0/rate) + period_sec=1.0 / rate) self._spot_wrapper = spot_wrapper @@ -183,8 +234,8 @@ def _start_query(self): # STATUS_AT_GOAL always means that the robot reached the goal. If the trajectory command did not # request precise positioning, then STATUS_NEAR_GOAL also counts as reaching the goal if status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL or \ - (status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL and - not self._spot_wrapper._last_trajectory_command_precise): + (status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL and + not self._spot_wrapper._last_trajectory_command_precise): self._spot_wrapper._at_goal = True # Clear the command once at the goal self._spot_wrapper._last_trajectory_command = None @@ -204,9 +255,11 @@ def _start_query(self): if self._spot_wrapper.is_standing and not self._spot_wrapper.is_moving: self._spot_wrapper.stand(False) + class SpotWrapper(): """Generic wrapper class to encompass release 1.1.4 API features as well as maintaining leases automatically""" - def __init__(self, username, password, hostname, logger, estop_timeout=9.0, rates = {}, callbacks = {}): + + def __init__(self, username, password, hostname, logger, estop_timeout=9.0, rates={}, callbacks={}): self._username = username self._password = password self._hostname = hostname @@ -241,6 +294,10 @@ def __init__(self, username, password, hostname, logger, estop_timeout=9.0, rate for source in rear_image_sources: self._rear_image_requests.append(build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW)) + self._hand_image_requests = [] + for source in hand_image_sources: + self._hand_image_requests.append(build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW)) + try: self._sdk = create_standard_sdk('ros_spot') except Exception as e: @@ -264,11 +321,13 @@ def __init__(self, username, password, hostname, logger, estop_timeout=9.0, rate self._robot_state_client = self._robot.ensure_client(RobotStateClient.default_service_name) self._robot_command_client = self._robot.ensure_client(RobotCommandClient.default_service_name) self._graph_nav_client = self._robot.ensure_client(GraphNavClient.default_service_name) + self._world_object_client = self._robot.ensure_client(WorldObjectClient.default_service_name) self._power_client = self._robot.ensure_client(PowerClient.default_service_name) self._lease_client = self._robot.ensure_client(LeaseClient.default_service_name) self._lease_wallet = self._lease_client.lease_wallet self._image_client = self._robot.ensure_client(ImageClient.default_service_name) self._estop_client = self._robot.ensure_client(EstopClient.default_service_name) + except Exception as e: self._logger.error("Unable to create client service: %s", e) self._valid = False @@ -276,25 +335,49 @@ def __init__(self, username, password, hostname, logger, estop_timeout=9.0, rate # Store the most recent knowledge of the state of the robot based on rpc calls. self._current_graph = None - self._current_edges = dict() #maps to_waypoint to list(from_waypoint) + self._current_edges = dict() # maps to_waypoint to list(from_waypoint) self._current_waypoint_snapshots = dict() # maps id to waypoint snapshot self._current_edge_snapshots = dict() # maps id to edge snapshot self._current_annotation_name_to_wp_id = dict() # Async Tasks self._async_task_list = [] - self._robot_state_task = AsyncRobotState(self._robot_state_client, self._logger, max(0.0, self._rates.get("robot_state", 0.0)), self._callbacks.get("robot_state", lambda:None)) - self._robot_metrics_task = AsyncMetrics(self._robot_state_client, self._logger, max(0.0, self._rates.get("metrics", 0.0)), self._callbacks.get("metrics", lambda:None)) - self._lease_task = AsyncLease(self._lease_client, self._logger, max(0.0, self._rates.get("lease", 0.0)), self._callbacks.get("lease", lambda:None)) - self._front_image_task = AsyncImageService(self._image_client, self._logger, max(0.0, self._rates.get("front_image", 0.0)), self._callbacks.get("front_image", lambda:None), self._front_image_requests) - self._side_image_task = AsyncImageService(self._image_client, self._logger, max(0.0, self._rates.get("side_image", 0.0)), self._callbacks.get("side_image", lambda:None), self._side_image_requests) - self._rear_image_task = AsyncImageService(self._image_client, self._logger, max(0.0, self._rates.get("rear_image", 0.0)), self._callbacks.get("rear_image", lambda:None), self._rear_image_requests) + self._robot_state_task = AsyncRobotState(self._robot_state_client, self._logger, + max(0.0, self._rates.get("robot_state", 0.0)), + self._callbacks.get("robot_state", lambda: None)) + self._localization_state_task = AsyncLocalizationState(self._graph_nav_client, self._logger, + max(0.0, self._rates.get("localization_state", 0.0)), + self._callbacks.get("localization_state", + lambda: None)) + self._robot_metrics_task = AsyncMetrics(self._robot_state_client, self._logger, + max(0.0, self._rates.get("metrics", 0.0)), + self._callbacks.get("metrics", lambda: None)) + self._lease_task = AsyncLease(self._lease_client, self._logger, max(0.0, self._rates.get("lease", 0.0)), + self._callbacks.get("lease", lambda: None)) + self._front_image_task = AsyncImageService(self._image_client, self._logger, + max(0.0, self._rates.get("front_image", 0.0)), + self._callbacks.get("front_image", lambda: None), + self._front_image_requests) + self._side_image_task = AsyncImageService(self._image_client, self._logger, + max(0.0, self._rates.get("side_image", 0.0)), + self._callbacks.get("side_image", lambda: None), + self._side_image_requests) + self._rear_image_task = AsyncImageService(self._image_client, self._logger, + max(0.0, self._rates.get("rear_image", 0.0)), + self._callbacks.get("rear_image", lambda: None), + self._rear_image_requests) + self._hand_image_task = AsyncImageService(self._image_client, self._logger, + max(0.0, self._rates.get("hand_image", 0.0)), + self._callbacks.get("hand_image", lambda: None), + self._hand_image_requests) self._idle_task = AsyncIdle(self._robot_command_client, self._logger, 10.0, self) self._estop_endpoint = None self._async_tasks = AsyncTasks( - [self._robot_state_task, self._robot_metrics_task, self._lease_task, self._front_image_task, self._side_image_task, self._rear_image_task, self._idle_task]) + [self._robot_state_task, self._localization_state_task, self._robot_metrics_task, self._lease_task, + self._front_image_task, self._side_image_task, self._rear_image_task, self._hand_image_task, + self._idle_task]) self._robot_id = None self._lease = None @@ -319,6 +402,11 @@ def robot_state(self): """Return latest proto from the _robot_state_task""" return self._robot_state_task.proto + @property + def localization_state(self): + """Return latest proto from the _localization_state_task""" + return self._localization_state_task.proto + @property def metrics(self): """Return latest proto from the _robot_metrics_task""" @@ -344,6 +432,11 @@ def rear_images(self): """Return latest proto from the _rear_image_task""" return self._rear_image_task.proto + @property + def hand_images(self): + """Return latest proto from the _hand_image_task""" + return self._hand_image_task.proto + @property def is_standing(self): """Return boolean of standing state""" @@ -451,7 +544,6 @@ def disengageEStop(self): except: return False, "Error" - def releaseEStop(self): """Stop eStop keepalive""" if self._estop_keepalive: @@ -495,7 +587,9 @@ def _robot_command(self, command_proto, end_time_secs=None, timesync_endpoint=No timesync_endpoint: (optional) Time sync endpoint """ try: - id = self._robot_command_client.robot_command(lease=None, command=command_proto, end_time_secs=end_time_secs, timesync_endpoint=timesync_endpoint) + id = self._robot_command_client.robot_command(lease=None, command=command_proto, + end_time_secs=end_time_secs, + timesync_endpoint=timesync_endpoint) return True, "Success", id except Exception as e: return False, str(e), None @@ -523,6 +617,24 @@ def stand(self, monitor_command=True): self._last_stand_command = response[2] return response[0], response[1] + def undock(self): + try: + blocking_undock(self._robot) + except CommandFailedError: + return False, "Undocking failed" + return True, "Undocking Succeeded" + + def dock(self, dock_id=520): + try: + # make sure we're powered on and standing + rospy.loginfo("docking at id {}".format(dock_id)) + self._robot.power_on() + robot_command.blocking_stand(self._robot_command_client) + blocking_dock_robot(self._robot, int(dock_id)) + except CommandFailedError: + return False, "Docking failed" + return True, "Docking Succeeded!" + def safe_power_off(self): """Stop the robot's motion and sit if possible. Once sitting, disable motor power.""" response = self._robot_command(RobotCommandBuilder.safe_power_off_command()) @@ -544,6 +656,28 @@ def power_on(self): except Exception as e: return False, str(e) + def list_tagged_objects(self): + request_fiducials = [world_object_pb2.WORLD_OBJECT_APRILTAG] + world_objects = self._world_object_client.list_world_objects(object_type=request_fiducials).world_objects + tagged_object_ids = [] + for world_obj in world_objects: + tagged_object_ids.append( + str(world_obj.apriltag_properties.frame_name_fiducial.split('_')[1])) + return tagged_object_ids + + def get_object_pose(self, id): + world_objects = self._world_object_client.list_world_objects().world_objects + for world_object in world_objects: + if id == str(world_object.id) or id == str(world_object.apriltag_properties.frame_name_fiducial.split('_')[1]): + # Get the transform snapshot for the world object + snapshot = world_object.transforms_snapshot + # Get the frame name for the fiducial of the object + fiducial_frame = world_object.apriltag_properties.frame_name_fiducial + # get the location of the fiducial in the odom frame + odom_tform_fiducial = get_a_tform_b(snapshot, "odom", fiducial_frame) + return True, odom_tform_fiducial + return False + def set_mobility_params(self, mobility_params): """Set Params for mobility and movement @@ -566,13 +700,61 @@ def velocity_cmd(self, v_x, v_y, v_rot, cmd_duration=0.125): v_rot: Angular velocity around the Z axis in radians cmd_duration: (optional) Time-to-live for the command in seconds. Default is 125ms (assuming 10Hz command rate). """ - end_time=time.time() + cmd_duration + end_time = time.time() + cmd_duration response = self._robot_command(RobotCommandBuilder.synchro_velocity_command( - v_x=v_x, v_y=v_y, v_rot=v_rot, params=self._mobility_params), - end_time_secs=end_time, timesync_endpoint=self._robot.time_sync.endpoint) + v_x=v_x, v_y=v_y, v_rot=v_rot, params=self._mobility_params), + end_time_secs=end_time, timesync_endpoint=self._robot.time_sync.endpoint) self._last_velocity_command_time = end_time return response[0], response[1] + def body_pose_cmd(self, goal_z, goal_rotation, cmd_duration, precise_position=False): + """Send a body pose command to the robot. + + Args: + goal_z: desired height of the robot (in the body frame) + goal_rotation: Quaternion representing the desired rotation (also in the body frame) + cmd_duration: Time-to-live for the command in seconds. + precise_position: if set to false, the status STATUS_NEAR_GOAL and STATUS_AT_GOAL will be equivalent. If + true, the robot must complete its final positioning before it will be considered to have successfully + reached the goal. + """ + self._at_goal = False + self._near_goal = False + self._last_trajectory_command_precise = precise_position + self._logger.info("got command duration of {}".format(cmd_duration)) + end_time = time.time() + cmd_duration + + # # transform into footprint frame: + # transforms = self._robot_state_client.get_robot_state().kinematic_state.transforms_snapshot + # # rospy.loginfo(transforms) + # gpe_tform_odom = get_a_tform_b(transforms, "gpe", "odom") + # odom_tform_goal = math_helpers.SE3Pose(x=0, y=0, z=goal_z, rot=goal_rotation) + # gpe_tform_goal = gpe_tform_odom * odom_tform_goal + # body_tform_odom = get_a_tform_b(transforms, "body", "odom") + # body_tform_goal = body_tform_odom * odom_tform_goal + + class JankyQuaternion: + def __init__(self, x, y, z, w): + self.x = x + self.y = y + self.z = z + self.w = w + + def to_quaternion(self): + return Quaternion(x=self.x, y=self.y, z=self.z, w=self.w) + + response = self._robot_command( + RobotCommandBuilder.synchro_stand_command(body_height=goal_z, + footprint_R_body=JankyQuaternion(goal_rotation.x, + goal_rotation.y, + goal_rotation.z, + goal_rotation.w)), + end_time_secs=end_time + ) + if response[0]: + self._last_trajectory_command = response[2] + return response[0], response[1] + def trajectory_cmd(self, goal_x, goal_y, goal_heading, cmd_duration, frame_name='odom', precise_position=False): """Send a trajectory motion command to the robot. @@ -590,35 +772,37 @@ def trajectory_cmd(self, goal_x, goal_y, goal_heading, cmd_duration, frame_name= self._near_goal = False self._last_trajectory_command_precise = precise_position self._logger.info("got command duration of {}".format(cmd_duration)) - end_time=time.time() + cmd_duration + end_time = time.time() + cmd_duration if frame_name == 'vision': vision_tform_body = frame_helpers.get_vision_tform_body( - self._robot_state_client.get_robot_state().kinematic_state.transforms_snapshot) - body_tform_goal = math_helpers.SE3Pose(x=goal_x, y=goal_y, z=0, rot=math_helpers.Quat.from_yaw(goal_heading)) + self._robot_state_client.get_robot_state().kinematic_state.transforms_snapshot) + body_tform_goal = math_helpers.SE3Pose(x=goal_x, y=goal_y, z=0, + rot=math_helpers.Quat.from_yaw(goal_heading)) vision_tform_goal = vision_tform_body * body_tform_goal response = self._robot_command( - RobotCommandBuilder.synchro_se2_trajectory_point_command( - goal_x=vision_tform_goal.x, - goal_y=vision_tform_goal.y, - goal_heading=vision_tform_goal.rot.to_yaw(), - frame_name=frame_helpers.VISION_FRAME_NAME, - params=self._mobility_params), - end_time_secs=end_time - ) + RobotCommandBuilder.synchro_se2_trajectory_point_command( + goal_x=vision_tform_goal.x, + goal_y=vision_tform_goal.y, + goal_heading=vision_tform_goal.rot.to_yaw(), + frame_name=frame_helpers.VISION_FRAME_NAME, + params=self._mobility_params), + end_time_secs=end_time + ) elif frame_name == 'odom': odom_tform_body = frame_helpers.get_odom_tform_body( - self._robot_state_client.get_robot_state().kinematic_state.transforms_snapshot) - body_tform_goal = math_helpers.SE3Pose(x=goal_x, y=goal_y, z=0, rot=math_helpers.Quat.from_yaw(goal_heading)) + self._robot_state_client.get_robot_state().kinematic_state.transforms_snapshot) + body_tform_goal = math_helpers.SE3Pose(x=goal_x, y=goal_y, z=0, + rot=math_helpers.Quat.from_yaw(goal_heading)) odom_tform_goal = odom_tform_body * body_tform_goal response = self._robot_command( - RobotCommandBuilder.synchro_se2_trajectory_point_command( - goal_x=odom_tform_goal.x, - goal_y=odom_tform_goal.y, - goal_heading=odom_tform_goal.rot.to_yaw(), - frame_name=frame_helpers.ODOM_FRAME_NAME, - params=self._mobility_params), - end_time_secs=end_time - ) + RobotCommandBuilder.synchro_se2_trajectory_point_command( + goal_x=odom_tform_goal.x, + goal_y=odom_tform_goal.y, + goal_heading=odom_tform_goal.rot.to_yaw(), + frame_name=frame_helpers.ODOM_FRAME_NAME, + params=self._mobility_params), + end_time_secs=end_time + ) else: raise ValueError('frame_name must be \'vision\' or \'odom\'') if response[0]: @@ -632,21 +816,10 @@ def list_graph(self, upload_path): """ ids, eds = self._list_graph_waypoint_and_edge_ids() # skip waypoint_ for v2.2.1, skip waypiont for < v2.2 - return [v for k, v in sorted(ids.items(), key=lambda id : int(id[0].replace('waypoint_','')))] + return [v for k, v in sorted(ids.items(), key=lambda id: int(id[0].replace('waypoint_', '')))] - def navigate_to(self, upload_path, - navigate_to, - initial_localization_fiducial=True, - initial_localization_waypoint=None): - """ navigate with graph nav. - - Args: - upload_path : Path to the root directory of the map. - navigate_to : Waypont id string for where to goal - initial_localization_fiducial : Tells the initializer whether to use fiducials - initial_localization_waypoint : Waypoint id string of current robot position (optional) - """ - # Filepath for uploading a saved graph's and snapshots too. + def localize_in_graph(self, upload_path, initial_localization_fiducial=True, initial_localization_waypoint = None): + # Filepath for uploading a saved graph's and snapshots to. if upload_path[-1] == "/": upload_filepath = upload_path[:-1] else: @@ -657,7 +830,7 @@ def navigate_to(self, upload_path, self._started_powered_on = (power_state.motor_power_state == power_state.STATE_ON) self._powered_on = self._started_powered_on - # FIX ME somehow,,,, if the robot is stand, need to sit the robot before starting garph nav + # FIX ME somehow,,,, if the robot is stand, need to sit the robot before starting graph nav if self.is_standing and not self.is_moving: self.sit() @@ -668,12 +841,508 @@ def navigate_to(self, upload_path, self._set_initial_localization_fiducial() if initial_localization_waypoint: self._set_initial_localization_waypoint([initial_localization_waypoint]) - self._list_graph_waypoint_and_edge_ids() - self._get_localization_state() + + localization_state = self._graph_nav_client.get_localization_state() + if not localization_state.localization.waypoint_id: + return False, "Localization failed!" + return True, "Localization succeeded!" + + def navigate_to(self, upload_path, + navigate_to, + initial_localization_fiducial=True, + initial_localization_waypoint=None): + """ navigate with graph nav. + + Args: + upload_path : Path to the root directory of the map. + navigate_to : Waypont id string for where to goal + initial_localization_fiducial : Tells the initializer whether to use fiducials + initial_localization_waypoint : Waypoint id string of current robot position (optional) + """ + # Filepath for uploading a saved graph's and snapshots too. + # self.localize_in_graph(upload_path, initial_localization_fiducial=initial_localization_fiducial, + # initial_localization_waypoint=initial_localization_waypoint) + # self._list_graph_waypoint_and_edge_ids() + # self._get_localization_state() resp = self._navigate_to([navigate_to]) return resp + # Arm ############################################ + def ensure_arm_power_and_stand(self): + if not self._robot.has_arm(): + return False, "Spot with an arm is required for this service" + + try: + self._logger.info("Spot is powering on within the timeout of 20 secs") + self._robot.power_on(timeout_sec=20) + assert self._robot.is_powered_on(), "Spot failed to power on" + self._logger.info("Spot is powered on") + except Exception as e: + return False, "Exception occured while Spot was trying to power on or stand" + + if not self._is_standing: + robot_command.blocking_stand(command_client=self._robot_command_client, timeout_sec=10.0) + self._logger.info("Spot is standing") + else: + self._logger.info("Spot is already standing") + + return True, "Spot has an arm, is powered on, and standing" + + def arm_stow(self): + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Stow Arm + stow = RobotCommandBuilder.arm_stow_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(stow) + self._logger.info("Command stow issued") + time.sleep(2.0) + + except Exception as e: + return False, "Exception occured while trying to stow" + + return True, "Stow arm success" + + def arm_unstow(self): + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Unstow Arm + unstow = RobotCommandBuilder.arm_ready_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(unstow) + self._logger.info("Command unstow issued") + time.sleep(2.0) + + except Exception as e: + return False, "Exception occured while trying to unstow" + + return True, "Unstow arm success" + + def arm_carry(self): + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Get Arm in carry mode + carry = RobotCommandBuilder.arm_carry_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(carry) + self._logger.info("Command carry issued") + time.sleep(2.0) + except Exception as e: + return False, "Exception occured while carry mode was issued" + return True, "Carry mode success" + + def make_arm_trajectory_command(self, arm_joint_trajectory): + """ Helper function to create a RobotCommand from an ArmJointTrajectory. + Copy from 'spot-sdk/python/examples/arm_joint_move/arm_joint_move.py' """ + + joint_move_command = arm_command_pb2.ArmJointMoveCommand.Request(trajectory=arm_joint_trajectory) + arm_command = arm_command_pb2.ArmCommand.Request(arm_joint_move_command=joint_move_command) + sync_arm = synchronized_command_pb2.SynchronizedCommand.Request(arm_command=arm_command) + arm_sync_robot_cmd = robot_command_pb2.RobotCommand(synchronized_command=sync_arm) + return RobotCommandBuilder.build_synchro_command(arm_sync_robot_cmd) + + def arm_joint_move(self, joint_targets): + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Joint1: 0.0 arm points to the front. RANGE: 0.0 -> 5.75959 (positive: turn left, negative: turn right) + # Joint2: 0.0 arm points to the front. RANGE: 0.0 -> 3.66519) + # Joint3: 0.0 arm straight. RANGE: 0.0 -> 3.1415 + # Joint4: 0.0 middle position. RANGE: -2.79253 -> 2.79253 + # Joint5: 0.0 gripper points to the front. RANGE: -1.8326 -> 1.8326 + # Joint6: 0.0 Moving finger on top of stationary finger. RANGE: -2.87979 -> 2.87979) + + trajectory_point = RobotCommandBuilder.create_arm_joint_trajectory_point( + joint_targets[0], joint_targets[1], joint_targets[2], + joint_targets[3], joint_targets[4], joint_targets[5]) + arm_joint_trajectory = arm_command_pb2.ArmJointTrajectory(points=[trajectory_point]) + arm_command = self.make_arm_trajectory_command(arm_joint_trajectory) + + # Send the request + cmd_id = self._robot_command_client.robot_command(arm_command) + + # Query for feedback to determine how long it will take + feedback_resp = self._robot_command_client.robot_command_feedback(cmd_id) + joint_move_feedback = feedback_resp.feedback.synchronized_feedback.arm_command_feedback.arm_joint_move_feedback + time_to_goal: Duration = joint_move_feedback.time_to_goal + time_to_goal_in_seconds: float = time_to_goal.seconds + (float(time_to_goal.nanos) / float(10 ** 9)) + time.sleep(time_to_goal_in_seconds) + return True, "Spot Arm moved successfully" + except Exception as e: + return False, "Exception occured during arm movement: " + str(e) + + def force_trajectory(self, forces_torques): + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Unstow arm + unstow = RobotCommandBuilder.arm_ready_command() + + # Send command via the RobotCommandClient + self._robot_command_client.robot_command(unstow) + + self._logger.info("Unstow command issued.") + time.sleep(2.0) + + # Demonstrate an example force trajectory by ramping up and down a vertical force over + # 10 seconds + + f_x0 = forces_torques[0] # Newtons + f_y0 = forces_torques[1] + f_z0 = forces_torques[2] + + f_x1 = forces_torques[3] # Newtons + f_y1 = forces_torques[4] + f_z1 = forces_torques[5] # -10 push down + + # We won't have any rotational torques + torque_x = forces_torques[6] + torque_y = forces_torques[7] + torque_z = forces_torques[8] + + # Duration in seconds. + traj_duration = 5 + + # First point of trajectory + force_vector0 = geometry_pb2.Vec3(x=f_x0, y=f_y0, z=f_z0) + torque_vector0 = geometry_pb2.Vec3(x=torque_x, y=torque_y, z=torque_z) + + wrench0 = geometry_pb2.Wrench(force=force_vector0, torque=torque_vector0) + t0 = seconds_to_duration(0) + traj_point0 = trajectory_pb2.WrenchTrajectoryPoint(wrench=wrench0, + time_since_reference=t0) + + # Second point on the trajectory + force_vector1 = geometry_pb2.Vec3(x=f_x1, y=f_y1, z=f_z1) + torque_vector1 = geometry_pb2.Vec3(x=torque_x, y=torque_y, z=torque_z) + + wrench1 = geometry_pb2.Wrench(force=force_vector1, torque=torque_vector1) + t1 = seconds_to_duration(traj_duration) + traj_point1 = trajectory_pb2.WrenchTrajectoryPoint(wrench=wrench1, + time_since_reference=t1) + + # Build the trajectory + self._logger.info("Building the trajectory") + trajectory = trajectory_pb2.WrenchTrajectory(points=[traj_point0, traj_point1]) + + # Build the trajectory request, putting all axes into force mode + arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( + root_frame_name=ODOM_FRAME_NAME, wrench_trajectory_in_task=trajectory, + x_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + y_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + z_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + rx_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + ry_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + rz_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE) + arm_command = arm_command_pb2.ArmCommand.Request( + arm_cartesian_command=arm_cartesian_command) + synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request( + arm_command=arm_command) + robot_command = robot_command_pb2.RobotCommand( + synchronized_command=synchronized_command) + + # Send the request + self._robot_command_client.robot_command(robot_command) + self._logger.info('Force trajectory command sent') + + time.sleep(10.0) + + except Exception as e: + return False, "Exception occured during arm movement" + str(e) + + def gripper_open(self): + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Open gripper + command = RobotCommandBuilder.claw_gripper_open_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(command) + self._logger.info("Command gripper open sent") + time.sleep(2.0) + + except Exception as e: + return False, "Exception occured while gripper was moving" + + return True, "Open gripper success" + + def gripper_close(self): + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Close gripper + command = RobotCommandBuilder.claw_gripper_close_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(command) + self._logger.info("Command gripper close sent") + time.sleep(2.0) + + except Exception as e: + return False, "Exception occurred while gripper was moving" + + return True, "Closed gripper successfully" + + def gripper_angle_open(self, gripper_ang): + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Open gripper at an angle + command = RobotCommandBuilder.claw_gripper_open_angle_command(gripper_q=gripper_ang) + + # Command issue with RobotCommandClient + cmd_id = self._robot_command_client.robot_command(command) + while True: + feedback_resp = self._robot_command_client.robot_command_feedback(cmd_id) + if feedback_resp.feedback.synchronized_feedback.gripper_command_feedback.claw_gripper_feedback.status == \ + gripper_command_pb2.ClawGripperCommand.Feedback.STATUS_AT_GOAL: + rospy.loginfo('Gripper move complete!') + break + time.sleep(0.1) + self._logger.info("Command gripper open angle sent") + # time.sleep(2.0) + + except Exception as e: + return False, "Exception occured while gripper was moving" + + return True, "Opened gripper successfully" + + def body_follow_arm(self): + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Move the arm to a spot in front of the robot, and command the body to follow the hand. + # Build a position to move the arm to (in meters, relative to the body frame origin.) + x = 1.25 + y = 0 + z = 0.25 + hand_pos_rt_body = geometry_pb2.Vec3(x=x, y=y, z=z) + + # Rotation as a quaternion. + qw = 1 + qx = 0 + qy = 0 + qz = 0 + body_Q_hand = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz) + + # Build the SE(3) pose of the desired hand position in the moving body frame. + body_T_hand = geometry_pb2.SE3Pose(position=hand_pos_rt_body, rotation=body_Q_hand) + + # Transform the desired from the moving body frame to the odom frame. + robot_state = self._robot_state_client.get_robot_state() + + odom_T_body = frame_helpers.get_a_tform_b(robot_state.kinematic_state.transforms_snapshot, + ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME) + odom_T_hand = odom_T_body * math_helpers.SE3Pose.from_obj(body_T_hand) + + # duration in seconds + seconds = 5 + + # Create the arm command. + arm_command = RobotCommandBuilder.arm_pose_command( + odom_T_hand.x, odom_T_hand.y, odom_T_hand.z, odom_T_hand.rot.w, odom_T_hand.rot.x, + odom_T_hand.rot.y, odom_T_hand.rot.z, ODOM_FRAME_NAME, seconds) + self._logger.info("Create arm command") + + # Tell the robot's body to follow the arm + follow_arm_command = RobotCommandBuilder.follow_arm_command() + + command = self._robot_command( + RobotCommandBuilder.build_synchro_command(follow_arm_command, arm_command)) + self._logger.info("After building command") + + # Send the request + self._robot_command_client.robot_command(command) + self._robot.logger.info('Moving arm to position.') + + time.sleep(6.0) + + except Exception as e: + return False, "Exception occured while arm was moving" + + return True, "Moved arm successfully" + + def hand_pose(self, pose_points, wrist_tform_tool=None): + try: + success, msg = self.ensure_arm_power_and_stand() + rospy.loginfo("In the server yo!") + if not success: + self._logger.info(msg) + return False, msg + else: + # Move the arm to a spot in front of the robot given a pose for the gripper. + # Build a position to move the arm to (in meters, relative to the body frame origin.) + x, y, z, qx, qy, qz, qw = pose_points + position = geometry_pb2.Vec3(x=x, y=y, z=z) + rotation = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz) + rospy.loginfo(position) + rospy.loginfo(rotation) + seconds = 6.0 + duration = seconds_to_duration(seconds) + rospy.loginfo("aight, we aboutta make the hand pose now") + # Build the SE(3) pose of the desired hand position in the moving body frame. + hand_pose = geometry_pb2.SE3Pose(position=position, rotation=rotation) + rospy.loginfo("finished this") + hand_pose_traj_point = trajectory_pb2.SE3TrajectoryPoint(pose=hand_pose, time_since_reference=duration) + rospy.loginfo("made traj point") + hand_trajectory = trajectory_pb2.SE3Trajectory(points=[hand_pose_traj_point]) + rospy.loginfo("made trajectory") + + # Build the SE(3) pose for wrist tform_tool (the default value was found in the protos) + # wx, wy, wz, wqw, wqx, wqy, wqz = wrist_tform_tool if not (wrist_tform_tool == None or wrist_tform_tool.length < 7) else None + # rospy.loginfo("unpacked wrist wform_tool") + # wtposition = geometry_pb2.Vec3(x=wx, y=wy, z=wz) + # rospy.loginfo("unpakced wtposition") + # wtrotation = geometry_pb2.Quaternion(w=wqw, x=wqx, y=wqy, z=wqz) + # rospy.loginfo("unpacked wt rotation") + # wrist_tform_tool = geometry_pb2.SE3Pose(position=wtposition, rotation=wtrotation) + rospy.loginfo("finished initializing command") + # proto stuff + arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( + root_frame_name=ODOM_FRAME_NAME, pose_trajectory_in_task=hand_trajectory) + # if not wrist_tform_tool == None: + # arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( + # root_frame_name=BODY_FRAME_NAME, pose_trajectory_in_task=hand_trajectory, + # wrist_tform_tool=wrist_tform_tool) + rospy.loginfo("built the cartesian command") + arm_command = arm_command_pb2.ArmCommand.Request( + arm_cartesian_command=arm_cartesian_command) + synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request( + arm_command=arm_command) + hand_pose_command = robot_command_pb2.RobotCommand(synchronized_command=synchronized_command) + # command = self._robot_command(RobotCommandBuilder.build_synchro_command(hand_pose_command)) + command = RobotCommandBuilder.build_synchro_command(hand_pose_command) + self._logger.info("After building command") + + # Send the request + rospy.loginfo("Moving arm to position {}, {}, {}".format(x, y, z)) + blocking_stand(self._robot_command_client, timeout_sec=10) + cmd_id = self._robot_command_client.robot_command(command) + rospy.loginfo("got the command ID") + rate = rospy.Rate(10) + while True: + feedback_resp = self._robot_command_client.robot_command_feedback(cmd_id) + rospy.loginfo( + 'Distance to final point: ' + '{:.2f} meters'.format( + feedback_resp.feedback.synchronized_feedback.arm_command_feedback. + arm_cartesian_feedback.measured_pos_distance_to_goal) + + ', {:.2f} radians'.format( + feedback_resp.feedback.synchronized_feedback.arm_command_feedback. + arm_cartesian_feedback.measured_rot_distance_to_goal)) + + if feedback_resp.feedback.synchronized_feedback.arm_command_feedback.arm_cartesian_feedback.status == arm_command_pb2.ArmCartesianCommand.Feedback.STATUS_TRAJECTORY_COMPLETE: + rospy.loginfo('Move complete.') + break + rate.sleep() + + except Exception as e: + return False, "Exception occured while arm was moving" + + return True, "Moved arm successfully" + + def walk_to_object_image(self, object_point): + print("In wrapper") + print(object_point) + + try: + success, msg = self.ensure_arm_power_and_stand() + self.gripper_open() + if not success: + self._logger.info(msg) + return False, msg + else: + walk_vec = geometry_pb2.Vec2(x=object_point[0], y=object_point[1]) + manipulation_api_client = self._robot.ensure_client(ManipulationApiClient.default_service_name) + print("manipulation_api_client") + + data = self.front_images + image_used = data[1] + + offset_distance = None + + # Build proto + walk_to = manipulation_api_pb2.WalkToObjectInImage( + pixel_xy=walk_vec, transforms_snapshot_for_camera=image_used.shot.transforms_snapshot, + frame_name_image_sensor=image_used.shot.frame_name_image_sensor, + camera_model=image_used.source.pinhole, offset_distance=offset_distance) + + print("Built proto") + + # Ask Spot to pick up the object + walk_to_request = manipulation_api_pb2.ManipulationApiRequest( + walk_to_object_in_image=walk_to) + + print("Walk to request") + + # Send the request + cmd_response = manipulation_api_client.manipulation_api_command( + manipulation_api_request=walk_to_request) + + print("Sent request") + + # Get feedback from robot + while True: + time.sleep(0.25) + feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( + manipulation_cmd_id=cmd_response.manipulation_cmd_id) + + print("Feedback request") + + # Send the request + response = manipulation_api_client.manipulation_api_feedback_command( + manipulation_api_feedback_request=feedback_request) + + print("Feedback response") + + print('Current state: ', + manipulation_api_pb2.ManipulationFeedbackState.Name(response.current_state)) + + if response.current_state == manipulation_api_pb2.MANIP_STATE_DONE: + break + + self._logger.info('Finished') + time.sleep(4.0) + + finally: + print("done") + + ################################################################### + ## copy from spot-sdk/python/examples/graph_nav_command_line/graph_nav_command_line.py def _get_localization_state(self, *args): """Get the current localization and state of the robot.""" @@ -716,8 +1385,8 @@ def _set_initial_localization_waypoint(self, *args): self._graph_nav_client.set_localization( initial_guess_localization=localization, # It's hard to get the pose perfect, search +/-20 deg and +/-20cm (0.2m). - max_distance = 0.2, - max_yaw = 20.0 * math.pi / 180.0, + max_distance=0.2, + max_yaw=20.0 * math.pi / 180.0, fiducial_init=graph_nav_pb2.SetLocalizationRequest.FIDUCIAL_INIT_NO_FIDUCIAL, ko_tform_body=current_odom_tform_body) @@ -738,7 +1407,6 @@ def _list_graph_waypoint_and_edge_ids(self, *args): graph, localization_id, self._logger) return self._current_annotation_name_to_wp_id, self._current_edges - def _upload_graph_and_snapshots(self, upload_filepath): """Upload the graph and snapshots to the robot.""" self._logger.info("Loading the graph from disk into local storage...") @@ -751,13 +1419,19 @@ def _upload_graph_and_snapshots(self, upload_filepath): len(self._current_graph.waypoints), len(self._current_graph.edges))) for waypoint in self._current_graph.waypoints: # Load the waypoint snapshots from disk. + file_name = os.path.join(upload_filepath, "waypoint_snapshots", waypoint.snapshot_id) + if not os.path.exists(file_name) or os.path.isdir(file_name): + continue with open(upload_filepath + "/waypoint_snapshots/{}".format(waypoint.snapshot_id), "rb") as snapshot_file: waypoint_snapshot = map_pb2.WaypointSnapshot() waypoint_snapshot.ParseFromString(snapshot_file.read()) self._current_waypoint_snapshots[waypoint_snapshot.id] = waypoint_snapshot for edge in self._current_graph.edges: - # Load the edge snapshots from disk. + # Load the edge snapshots from disk + file_name = os.path.join(upload_filepath, "edge_snapshots", edge.snapshot_id) + if not os.path.exists(file_name) or os.path.isdir(file_name): + continue with open(upload_filepath + "/edge_snapshots/{}".format(edge.snapshot_id), "rb") as snapshot_file: edge_snapshot = map_pb2.EdgeSnapshot() @@ -782,8 +1456,8 @@ def _upload_graph_and_snapshots(self, upload_filepath): if not localization_state.localization.waypoint_id: # The robot is not localized to the newly uploaded graph. self._logger.info( - "Upload complete! The robot is currently not localized to the map; please localize", \ - "the robot using commands (2) or (3) before attempting a navigation command.") + "Upload complete! The robot is currently not localized to the map; please localize" + + "the robot using commands (2) or (3) before attempting a navigation command.") def _navigate_to(self, *args): """Navigate to a specific waypoint.""" @@ -799,6 +1473,7 @@ def _navigate_to(self, *args): if not destination_waypoint: # Failed to find the appropriate unique waypoint id for the navigation command. return + rospy.loginfo("Naivgating to waypoint {}".format(destination_waypoint)) if not self.toggle_power(should_power_on=True): self._logger.info("Failed to power on the robot, and cannot complete navigate to request.") return @@ -918,7 +1593,7 @@ def toggle_power(self, should_power_on): motors_on = False while not motors_on: future = self._robot_state_client.get_robot_state_async() - state_response = future.result(timeout=10) # 10 second timeout for waiting for the state response. + state_response = future.result(timeout=10) # 10 second timeout for waiting for the state response. if state_response.power_state.motor_power_state == robot_state_pb2.PowerState.STATE_ON: motors_on = True else: diff --git a/spot_msgs/CMakeLists.txt b/spot_msgs/CMakeLists.txt index 9bbcd033..40138bfa 100644 --- a/spot_msgs/CMakeLists.txt +++ b/spot_msgs/CMakeLists.txt @@ -30,7 +30,9 @@ add_message_files( FootState.msg Lease.msg LeaseResource.msg + LocalizationState.msg PowerState.msg + ManipulatorState.msg SystemFaultState.msg ) @@ -40,12 +42,22 @@ add_service_files( SetLocomotion.srv SetVelocity.srv ClearBehaviorFault.srv + ListTaggedObjects.srv + GetObjectPose.srv + ArmJointMovement.srv + ConstrainedArmJointMovement.srv + GripperAngleMove.srv + ArmForceTrajectory.srv + HandPose.srv + LocalizeInGraph.srv + DockRobot.srv ) add_action_files( FILES NavigateTo.action Trajectory.action + GripperAngle.action ) generate_messages( diff --git a/spot_msgs/action/GripperAngle.action b/spot_msgs/action/GripperAngle.action new file mode 100644 index 00000000..2c0a25be --- /dev/null +++ b/spot_msgs/action/GripperAngle.action @@ -0,0 +1,5 @@ +float64 gripper_angle +--- +bool success +--- +float64 percent_completes \ No newline at end of file diff --git a/spot_msgs/action/NavigateTo.action b/spot_msgs/action/NavigateTo.action index 6450c01f..6f2f9865 100644 --- a/spot_msgs/action/NavigateTo.action +++ b/spot_msgs/action/NavigateTo.action @@ -6,5 +6,6 @@ string initial_localization_waypoint # Waypoint id to trigger localization bool success # indicate successful run of triggered service string message # informational, e.g. for error messages --- +int32 status string waypoint_id diff --git a/spot_msgs/msg/LocalizationState.msg b/spot_msgs/msg/LocalizationState.msg new file mode 100644 index 00000000..ae2a4451 --- /dev/null +++ b/spot_msgs/msg/LocalizationState.msg @@ -0,0 +1,5 @@ +bool is_localized +string waypoint_id +geometry_msgs/PoseStamped waypoint_tform_body +geometry_msgs/PoseStamped seed_tform_body +time timestamp \ No newline at end of file diff --git a/spot_msgs/msg/ManipulatorState.msg b/spot_msgs/msg/ManipulatorState.msg new file mode 100644 index 00000000..f8611fff --- /dev/null +++ b/spot_msgs/msg/ManipulatorState.msg @@ -0,0 +1,10 @@ +uint8 STOWSTATE_UNKNOWN = 0 +uint8 STOWSTATE_STOWED = 1 +uint8 STOWSTATE_DEPLOYED = 2 + +float32 gripper_open_percentage +bool is_gripper_holding_item +geometry_msgs/Vector3 estimated_end_effector_force_in_hand +uint8 stow_state +geometry_msgs/Twist velocity_of_hand_in_vision +geometry_msgs/Twist velocity_of_hand_in_odom diff --git a/spot_msgs/srv/ArmForceTrajectory.srv b/spot_msgs/srv/ArmForceTrajectory.srv new file mode 100644 index 00000000..a754adc5 --- /dev/null +++ b/spot_msgs/srv/ArmForceTrajectory.srv @@ -0,0 +1,4 @@ +float64[] force_torque +--- +bool success +string message diff --git a/spot_msgs/srv/ArmJointMovement.srv b/spot_msgs/srv/ArmJointMovement.srv new file mode 100644 index 00000000..d6085648 --- /dev/null +++ b/spot_msgs/srv/ArmJointMovement.srv @@ -0,0 +1,4 @@ +float64[] joint_target +--- +bool success +string message \ No newline at end of file diff --git a/spot_msgs/srv/ConstrainedArmJointMovement.srv b/spot_msgs/srv/ConstrainedArmJointMovement.srv new file mode 100644 index 00000000..9516881b --- /dev/null +++ b/spot_msgs/srv/ConstrainedArmJointMovement.srv @@ -0,0 +1,7 @@ +float64[] joint_target +float64 max_execution_time +float64 max_velocity +float64 max_acceleration +--- +bool success +string message \ No newline at end of file diff --git a/spot_msgs/srv/DockRobot.srv b/spot_msgs/srv/DockRobot.srv new file mode 100644 index 00000000..1d54edec --- /dev/null +++ b/spot_msgs/srv/DockRobot.srv @@ -0,0 +1,4 @@ +string id +--- +bool success +string message \ No newline at end of file diff --git a/spot_msgs/srv/GetObjectPose.srv b/spot_msgs/srv/GetObjectPose.srv new file mode 100644 index 00000000..90974b32 --- /dev/null +++ b/spot_msgs/srv/GetObjectPose.srv @@ -0,0 +1,5 @@ +string id +--- +bool success +string message +geometry_msgs/PoseStamped object_pose \ No newline at end of file diff --git a/spot_msgs/srv/GripperAngleMove.srv b/spot_msgs/srv/GripperAngleMove.srv new file mode 100644 index 00000000..a3c2cefe --- /dev/null +++ b/spot_msgs/srv/GripperAngleMove.srv @@ -0,0 +1,4 @@ +float64 gripper_angle +--- +bool success +string message diff --git a/spot_msgs/srv/HandPose.srv b/spot_msgs/srv/HandPose.srv new file mode 100644 index 00000000..ef2e8ff6 --- /dev/null +++ b/spot_msgs/srv/HandPose.srv @@ -0,0 +1,5 @@ +float64[] pose_point +float64[] wrist_tform_tool +--- +bool success +string message diff --git a/spot_msgs/srv/ListTaggedObjects.srv b/spot_msgs/srv/ListTaggedObjects.srv new file mode 100644 index 00000000..ec7368e6 --- /dev/null +++ b/spot_msgs/srv/ListTaggedObjects.srv @@ -0,0 +1,2 @@ +--- +string[] waypoint_ids \ No newline at end of file diff --git a/spot_msgs/srv/LocalizeInGraph.srv b/spot_msgs/srv/LocalizeInGraph.srv new file mode 100644 index 00000000..da834844 --- /dev/null +++ b/spot_msgs/srv/LocalizeInGraph.srv @@ -0,0 +1,6 @@ +string upload_path # Absolute path to map_directory, which is downloaded from tablet controller +bool initial_localization_fiducial # Tells the initializer whether to use fiducials +string initial_localization_waypoint # Waypoint id to trigger localization +--- +bool success # indicate successful run of triggered service +string message # informational, e.g. for error messages \ No newline at end of file