├── .gitignore ├── README.md ├── ansible ├── README.md ├── inventory ├── roles │ ├── ros_user │ │ ├── files │ │ │ └── logrotate-ros.conf │ │ └── tasks │ │ │ └── main.yml │ ├── ubr_ros1 │ │ ├── files │ │ │ ├── melodic.rosinstall │ │ │ └── noetic.rosinstall │ │ ├── tasks │ │ │ └── main.yml │ │ └── templates │ │ │ ├── robot.j2 │ │ │ └── roscore.j2 │ └── ubr_ros2 │ │ ├── files │ │ └── jazzy.repos │ │ ├── tasks │ │ └── main.yml │ │ └── templates │ │ ├── cyclonedds.j2 │ │ ├── cyclonesysctl.j2 │ │ └── robot.j2 ├── ubr_jazzy.yml └── ubr_ros1.yml ├── docker ├── Dockerfile ├── README.md └── ros_entrypoint.sh ├── ubr1_bringup ├── CMakeLists.txt ├── config │ ├── default_controllers.yaml │ └── fixed_lag_smoother.yaml ├── launch │ ├── head_camera.launch.py │ └── robot.launch.py ├── package.xml └── scripts │ ├── controller_reset.py │ ├── profile_base.py │ └── test_accelerations.py ├── ubr1_calibration ├── CMakeLists.txt ├── README.md ├── config │ ├── calibrate.yaml │ ├── calibrate_base.yaml │ ├── calibration_poses.yaml │ ├── camera_in_hand │ │ ├── calibrate.yaml │ │ └── capture.yaml │ ├── capture.yaml │ └── checkerboard_2d │ │ ├── calibrate.yaml │ │ └── capture.yaml ├── launch │ ├── calibrate.launch.py │ └── calibrate_base.launch.py ├── package.xml └── scripts │ ├── calibrate_from_bag │ ├── calibrate_manually │ ├── camera_in_hand │ └── camera_in_hand_manually │ ├── camera_reconfigure.py │ └── checkerboard_2d │ └── checkerboard_2d_manually ├── ubr1_demo ├── CMakeLists.txt ├── include │ └── ubr1_demo │ │ ├── generate_grasps_from_msg.hpp │ │ └── pick_place_task.hpp ├── launch │ ├── pick_place.launch.py │ └── simple_grasping.launch.py ├── package.xml └── src │ ├── generate_grasps_from_msg.cpp │ ├── pick_and_place.cpp │ └── pick_place_task.cpp ├── ubr1_description ├── CMakeLists.txt ├── meshes │ ├── base_laser_link.STL │ ├── base_link.STL │ ├── base_link_col.STL │ ├── bellows_link.STL │ ├── bellows_link_col.STL │ ├── elbow_flex_link.STL │ ├── estop_link.STL │ ├── fixed_torso_link.STL │ ├── forearm_roll_link.STL │ ├── forearm_roll_link_col.STL │ ├── gripper_link.STL │ ├── gripper_link_col.STL │ ├── head_camera_link.STL │ ├── head_pan_link.STL │ ├── head_tilt_link.STL │ ├── left_gripper_finger_link.STL │ ├── left_wheel_link.STL │ ├── right_gripper_finger_link.STL │ ├── right_wheel_link.STL │ ├── shoulder_lift_link.STL │ ├── shoulder_lift_link_col.STL │ ├── shoulder_pan_link.STL │ ├── shoulder_pan_link_col.STL │ ├── torso_lift_link.STL │ ├── torso_lift_link_col.STL │ ├── upperarm_roll_link.STL │ ├── upperarm_roll_link_col.STL │ └── wrist_flex_link.STL ├── package.xml └── robots │ └── ubr1_robot.urdf ├── ubr1_gazebo ├── CMakeLists.txt ├── COLCON_IGNORE ├── config │ └── default_controllers.yaml ├── include │ └── ubr1_gazebo │ │ ├── joint_handle.h │ │ ├── simulated_bellows_controller.h │ │ ├── simulated_gripper_controller.h │ │ └── ubr1_gazebo_plugin.h ├── launch │ ├── include │ │ ├── head_camera.launch.xml │ │ └── simulation.ubr1.xml │ ├── simulation.launch │ ├── simulation_grasping.launch │ └── simulation_home.launch ├── models │ ├── table_30 │ │ ├── model.config │ │ └── table.sdf │ └── ubr_cube │ │ ├── model.config │ │ └── ubr_cube.sdf ├── package.xml ├── robots │ └── ubr1_robot.urdf.xacro ├── scripts │ └── stop_controllers.py ├── src │ ├── simulated_bellows_controller.cpp │ ├── simulated_gripper_controller.cpp │ └── ubr1_gazebo_plugin.cpp ├── test │ ├── pick_up_cube_test.launch │ ├── pick_up_cube_test.py │ ├── pick_up_cube_trajectories.bag │ └── scripts │ │ └── generate_grasp_test.py ├── ubr1_gazebo_controllers.xml └── worlds │ ├── cube_on_ground.sdf │ ├── cube_on_table.sdf │ └── home.sdf ├── ubr1_moveit ├── .setup_assistant ├── CMakeLists.txt ├── config │ ├── initial_positions.yaml │ ├── joint_limits.yaml │ ├── kinematics.yaml │ ├── moveit.rviz │ ├── moveit_controllers.yaml │ ├── pilz_cartesian_limits.yaml │ ├── ros2_controllers.yaml │ ├── sensors_3d.yaml │ ├── ubr1.ros2_control.xacro │ ├── ubr1.srdf │ └── ubr1.urdf.xacro ├── launch │ ├── demo.launch.py │ ├── move_group.launch.py │ ├── moveit_rviz.launch.py │ ├── rsp.launch.py │ ├── setup_assistant.launch.py │ ├── spawn_controllers.launch.py │ └── static_virtual_joint_tfs.launch.py └── package.xml ├── ubr1_navigation ├── CMakeLists.txt ├── behavior_trees │ └── default.xml ├── config │ ├── mapper_params_online_sync.yaml │ └── nav2_params.yaml ├── costmap_plugins.xml ├── include │ └── ubr1_navigation │ │ └── depth_layer.hpp ├── launch │ ├── build_map.launch.py │ ├── includes │ │ └── move_base.launch.xml │ ├── localization.launch.py │ └── navigation.launch.py ├── maps │ ├── map.pgm │ ├── map.yaml │ ├── map_1728569604.pgm │ ├── map_1728569604.yaml │ ├── map_with_arena.pgm │ └── map_with_arena.yaml ├── package.xml ├── scripts │ └── tilt_head.py ├── src │ └── depth_layer.cpp └── srv │ └── TiltControl.srv ├── ubr_msgs ├── CMakeLists.txt ├── msg │ ├── BoardInfo.msg │ ├── BreakerInfo.msg │ ├── BreakerState.msg │ ├── ChargerInfo.msg │ ├── DebugInfo.msg │ ├── JointInfo.msg │ ├── MotorInfo.msg │ └── RobotState.msg ├── package.xml └── srv │ └── BreakerCommand.srv └── ubr_teleop ├── CMakeLists.txt ├── include └── ubr_teleop │ └── robot_controller.h ├── package.xml ├── scripts ├── set_gripper_pose.py ├── set_head_pose.py ├── set_torso_pose.py └── tuck_arm.py └── src ├── joystick_teleop.cpp └── keyboard_teleop.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *__pycache__ 2 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | UBR-1 Reloaded 2 | ============== 3 | This is a fork of the ubr1_preview repo, updated to work with an actual robot 4 | on newer versions of ROS. 5 | 6 | The closest thing to documentation would be to read the 7 | [posts about the UBR-1 on my blog](https://www.robotandchisel.com/tag/ubr1/) 8 | 9 | There are some specific notes on calibration (using `robot_calibration`) 10 | in the [ubr1_calibration README](https://github.com/mikeferguson/ubr_reloaded/blob/ros2/ubr1_calibration/README.md) 11 | 12 | ## Launch Files 13 | 14 | ### ubr1_bringup/robot.launch.py 15 | This launches the full robot with drivers and teleoperation. 16 | Usually done as part of a system service installed via ansible. 17 | 18 | ### ubr1_navigation/build_map.launch.py 19 | Used to build a map with ``slam_toolbox``. 20 | Can not be run at the same time as ``localization.launch.py``. 21 | 22 | ### ubr1_navigation/localization.launch.py 23 | Launches ``amcl`` node, configured for UBR-1. 24 | 25 | ### ubr1_navigation/navigation.launch.py 26 | Launches the navigation stack. 27 | Requires ``localization.launch.py`` to also be running. 28 | 29 | ### ubr1_moveit/move_group.launch.py 30 | Launches MoveIt2 move_group, with the MoveIt Task Constructor. 31 | 32 | ### ubr1_demo/simple_grasping.launch.py 33 | Launches simple grasping's ``basic_grasping_perception`` node, 34 | properly configured for the UBR-1 gripper. 35 | 36 | ### ubr1_demo/pick_place.launch.py 37 | Launches the pick and place demo using MTC. 38 | -------------------------------------------------------------------------------- /ansible/README.md: -------------------------------------------------------------------------------- 1 | # Configuring the Robot 2 | 3 | Setup networking through the GUI: 4 | 5 | IPv4 Method: Manual 6 | IP: 10.42.0.1 7 | Netmask: 255.255.255.0 8 | Check for "Use this connection only for resources on its network" 9 | 10 | 11 | Run the playbook: 12 | 13 | ansible-playbook ubr_ros1.yml -i inventory --ask-become-pass 14 | ansible-playbook ubr_jazzy.yml -i inventory --ask-become-pass 15 | -------------------------------------------------------------------------------- /ansible/inventory: -------------------------------------------------------------------------------- 1 | all: 2 | hosts: 3 | robot: 4 | ansible_host: 127.0.0.1 5 | vars: 6 | ansible_connection: local 7 | -------------------------------------------------------------------------------- /ansible/roles/ros_user/files/logrotate-ros.conf: -------------------------------------------------------------------------------- 1 | var/log/ros/robot.log { 2 | daily 3 | rotate 3 4 | size 50M 5 | compress 6 | missingok 7 | } 8 | -------------------------------------------------------------------------------- /ansible/roles/ros_user/tasks/main.yml: -------------------------------------------------------------------------------- 1 | --- 2 | # Setup a "ros" user to run systemd jobs 3 | - name: Create the ROS user 4 | user: 5 | name: ros 6 | groups: audio, dialout, plugdev 7 | comment: ROS System User 8 | shell: /bin/bash 9 | home: /var/lib/ros 10 | create_home: no 11 | system: yes 12 | become: yes 13 | 14 | - name: Create /var/lib/ros 15 | file: 16 | path: /var/lib/ros 17 | state: directory 18 | owner: ros 19 | group: ros 20 | mode: '2775' 21 | become: yes 22 | 23 | - name: Create /var/log/ros 24 | file: 25 | path: /var/log/ros 26 | state: directory 27 | owner: ros 28 | group: ros 29 | mode: '2775' 30 | become: yes 31 | 32 | - name: Create robot.log 33 | file: 34 | path: /var/log/ros/robot.log 35 | state: touch 36 | owner: ros 37 | group: ros 38 | mode: '2775' 39 | become: yes 40 | 41 | - name: Rotate the robot.log 42 | copy: 43 | src: files/logrotate-ros.conf 44 | dest: /etc/logrotate.d/ros.conf 45 | become: yes 46 | -------------------------------------------------------------------------------- /ansible/roles/ubr_ros1/files/melodic.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: ubr_drivers 3 | uri: git@github.com:mikeferguson/ubr_drivers.git 4 | version: ros1 5 | - git: 6 | local-name: ubr_reloaded 7 | uri: git@github.com:mikeferguson/ubr_reloaded.git 8 | version: ros1 9 | -------------------------------------------------------------------------------- /ansible/roles/ubr_ros1/files/noetic.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: joystick_drivers 3 | uri: https://github.com/ros-drivers/joystick_drivers 4 | version: master 5 | - git: 6 | local-name: openni2_camera 7 | uri: https://github.com/ros-drivers/openni2_camera.git 8 | version: indigo-devel 9 | - git: 10 | local-name: robot_calibration 11 | uri: git@github.com:mikeferguson/robot_calibration.git 12 | version: ros1 13 | - git: 14 | local-name: robot_controllers 15 | uri: git@github.com:mikeferguson/robot_controllers.git 16 | version: ros1 17 | - git: 18 | local-name: ubr_drivers 19 | uri: git@github.com:mikeferguson/ubr_drivers.git 20 | version: ros1 21 | - git: 22 | local-name: ubr_reloaded 23 | uri: git@github.com:mikeferguson/ubr_reloaded.git 24 | version: ros1 25 | - git: 26 | local-name: urg_node 27 | uri: https://github.com/ros-drivers/urg_node.git 28 | version: indigo-devel 29 | -------------------------------------------------------------------------------- /ansible/roles/ubr_ros1/tasks/main.yml: -------------------------------------------------------------------------------- 1 | --- 2 | # ROS Setup 3 | - name: Configure apt key for ROS 4 | apt_key: 5 | keyserver: keyserver.ubuntu.com 6 | id: C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 7 | become: yes 8 | 9 | - name: Configure apt sources for ROS 10 | apt_repository: 11 | repo: 'deb http://packages.ros.org/ros/ubuntu {{ ansible_lsb.codename }} main' 12 | filename: 'ros-latest' 13 | update_cache: yes 14 | become: yes 15 | 16 | - name: Install base level ros packages 17 | apt: 18 | pkg: ros-{{rosdistro}}-desktop-full 19 | become: yes 20 | 21 | # Workspace setup 22 | - name: Install wstool (Python 3) 23 | apt: 24 | name: python3-wstool 25 | when: rosdistro == "noetic" 26 | become: yes 27 | 28 | - name: Install wstool (Python 2) 29 | apt: 30 | name: python-wstool 31 | when: rosdistro != "noetic" 32 | become: yes 33 | 34 | - name: Create workspace 35 | file: 36 | path: "{{ workspace_root }}/src" 37 | state: directory 38 | owner: ubr 39 | group: ubr 40 | mode: '2775' 41 | 42 | - name: Init wstool 43 | shell: wstool init src 44 | args: 45 | chdir: "{{ workspace_root }}" 46 | creates: "{{ workspace_root }}/src/.rosinstall" 47 | 48 | - name: Merge wstool 49 | shell: wstool merge -t src -a -y "{{ role_path }}/files/{{ rosdistro }}.rosinstall" 50 | args: 51 | chdir: "{{ workspace_root }}" 52 | 53 | - name: Update code 54 | shell: wstool update -t src --delete-changed-uris 55 | args: 56 | chdir: "{{ workspace_root }}" 57 | 58 | - name: Disable joystick_drivers metapackage 59 | file: 60 | path: "{{ workspace_root }}/src/joystick_drivers/joystick_drivers/CATKIN_IGNORE" 61 | state: touch 62 | owner: ubr 63 | group: ubr 64 | 65 | - name: Disable ubr1_gazebo (not currently building right) 66 | file: 67 | path: "{{ workspace_root }}/src/ubr_reloaded/ubr1_gazebo/CATKIN_IGNORE" 68 | state: touch 69 | owner: ubr 70 | group: ubr 71 | when: rosdistro == "noetic" 72 | 73 | - name: Install dependencies 74 | shell: rosdep update && rosdep install --from-paths src --ignore-src --rosdistro={{ rosdistro }} -y -r 75 | args: 76 | chdir: "{{ workspace_root }}" 77 | 78 | - name: Build code 79 | shell: catkin_make -DCMAKE_BUILD_TYPE=Release 80 | args: 81 | chdir: "{{ workspace_root }}" 82 | 83 | # Launch setup 84 | - name: Create /etc/ros/distro 85 | file: 86 | path: /etc/ros/{{ rosdistro }} 87 | state: directory 88 | owner: ros 89 | group: ros 90 | mode: '2775' 91 | become: yes 92 | 93 | - name: Copy launch file to /etc/ros 94 | copy: 95 | src: "{{ workspace_root }}/src/ubr_reloaded/ubr1_bringup/launch/robot.launch" 96 | dest: "/etc/ros/{{ rosdistro }}/robot.launch" 97 | force: no 98 | become: yes 99 | 100 | - name: Create a systemd service for roscore 101 | template: 102 | src: roscore.j2 103 | dest: /etc/systemd/system/roscore.service 104 | 105 | - name: Start the roscore service 106 | systemd: 107 | name: roscore 108 | enabled: yes 109 | state: started 110 | daemon_reload: yes 111 | become: yes 112 | 113 | - name: Create a systemd service for robot drivers 114 | template: 115 | src: robot.j2 116 | dest: /etc/systemd/system/robot.service 117 | become: yes 118 | 119 | - name: Start the robot service 120 | systemd: 121 | name: robot 122 | enabled: yes 123 | state: started 124 | daemon_reload: yes 125 | become: yes 126 | 127 | # Hardware config 128 | - name: Power off when button pressed 129 | shell: hostnamectl set-chassis vm 130 | become: yes 131 | 132 | - name: Hack to make python scripts work without changing shebang 133 | file: 134 | path: /usr/bin/python 135 | src: /usr/bin/python3 136 | state: link 137 | owner: root 138 | group: root 139 | mode: '777' 140 | become: yes 141 | -------------------------------------------------------------------------------- /ansible/roles/ubr_ros1/templates/robot.j2: -------------------------------------------------------------------------------- 1 | 2 | [Unit] 3 | Description=Start UBR-1 drivers 4 | After=roscore.service 5 | BindsTo=roscore.service 6 | 7 | [Install] 8 | WantedBy=roscore.service 9 | 10 | [Service] 11 | Environment="ROS_LOG_DIR=/var/log/ros" 12 | ExecStart=/bin/bash -c ". {{ workspace_root}}/devel/setup.bash && roslaunch /etc/ros/{{rosdistro}}/robot.launch --wait" 13 | StandardOutput=file:/var/log/ros/robot.log 14 | StandardError=file:/var/log/ros/robot.log 15 | User=ros 16 | -------------------------------------------------------------------------------- /ansible/roles/ubr_ros1/templates/roscore.j2: -------------------------------------------------------------------------------- 1 | [Unit] 2 | Description=Start roscore through systemd 3 | 4 | [Service] 5 | Environment="ROS_LOG_DIR=/var/log/ros" 6 | ExecStart=/bin/bash -c ". /opt/ros/{{rosdistro}}/setup.bash && roscore" 7 | User=ros 8 | 9 | [Install] 10 | WantedBy=multi-user.target 11 | -------------------------------------------------------------------------------- /ansible/roles/ubr_ros2/files/jazzy.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | graceful_controller: 3 | type: git 4 | url: git@github.com:mikeferguson/graceful_controller.git 5 | version: ros2 6 | grasping_msgs: 7 | type: git 8 | url: git@github.com:mikeferguson/grasping_msgs.git 9 | version: ros2 10 | robot_controllers: 11 | type: git 12 | url: git@github.com:mikeferguson/robot_controllers.git 13 | version: fix_cancel 14 | simple_grasping: 15 | type: git 16 | url: git@github.com:mikeferguson/simple_grasping.git 17 | version: ros2 18 | ubr_drivers: 19 | type: git 20 | url: git@github.com:mikeferguson/ubr_drivers.git 21 | version: ros2 22 | ubr_reloaded: 23 | type: git 24 | url: git@github.com:mikeferguson/ubr_reloaded.git 25 | version: ros2 26 | -------------------------------------------------------------------------------- /ansible/roles/ubr_ros2/tasks/main.yml: -------------------------------------------------------------------------------- 1 | --- 2 | # ROS2 Setup 3 | - name: Configure apt key for ROS2 4 | apt_key: 5 | keyserver: keyserver.ubuntu.com 6 | id: C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 7 | become: yes 8 | 9 | - name: Configure apt sources for ROS2 10 | apt_repository: 11 | repo: 'deb [arch=amd64,arm64] http://packages.ros.org/ros2/ubuntu {{ ansible_lsb.codename }} main' 12 | filename: 'ros2-latest' 13 | update_cache: yes 14 | become: yes 15 | 16 | - name: Install base level ROS2 packages 17 | apt: 18 | pkg: ros-{{rosdistro}}-desktop 19 | become: yes 20 | 21 | # Workspace setup 22 | - name: Install vcstool 23 | apt: 24 | pkg: python3-vcstool 25 | become: yes 26 | 27 | - name: Install rosdep 28 | apt: 29 | pkg: python3-rosdep 30 | become: yes 31 | 32 | - name: Install colcon 33 | apt: 34 | pkg: python3-colcon-common-extensions 35 | become: yes 36 | 37 | - name: Create workspace 38 | file: 39 | path: "{{ workspace_root }}/src" 40 | state: directory 41 | owner: ubr 42 | group: ubr 43 | mode: '2775' 44 | 45 | - name: Setup workspace with vcstool 46 | shell: "vcs import src < {{ role_path }}/files/{{ rosdistro }}.repos" 47 | args: 48 | chdir: "{{ workspace_root }}" 49 | 50 | - name: Update workspace with vcstool 51 | shell: "vcs pull src" 52 | args: 53 | chdir: "{{ workspace_root }}" 54 | 55 | - name: Install dependencies 56 | shell: rosdep update && rosdep install --from-paths src --ignore-src --rosdistro={{ rosdistro }} -y -r 57 | args: 58 | chdir: "{{ workspace_root }}" 59 | 60 | - name: Build workspace 61 | shell: source /opt/ros/{{ rosdistro }}/setup.bash && colcon build --symlink-install 62 | args: 63 | chdir: "{{ workspace_root }}" 64 | 65 | # Launch setup 66 | - name: Create /etc/ros/distro 67 | file: 68 | path: /etc/ros/{{ rosdistro }} 69 | state: directory 70 | owner: ros 71 | group: ros 72 | mode: '2775' 73 | become: yes 74 | 75 | - name: Copy launch file to /etc/ros 76 | copy: 77 | src: "{{ workspace_root }}/src/ubr_reloaded/ubr1_bringup/launch/robot.launch.py" 78 | dest: "/etc/ros/{{ rosdistro }}/robot.launch.py" 79 | force: no 80 | mode: '0755' 81 | become: yes 82 | 83 | - name: Setup kernel parameters for Cyclone DDS 84 | template: 85 | src: cyclonesysctl.j2 86 | dest: /etc/sysctl.d/10-cyclone-max.conf 87 | become: yes 88 | 89 | - name: Create the Cyclone DDS config 90 | template: 91 | src: cyclonedds.j2 92 | dest: "/etc/ros/{{ rosdistro }}/cyclonedds.xml" 93 | become: yes 94 | 95 | - name: Create a systemd service for robot drivers 96 | template: 97 | src: robot.j2 98 | dest: /etc/systemd/system/robot.service 99 | become: yes 100 | 101 | - name: Start the robot service 102 | systemd: 103 | name: robot 104 | enabled: yes 105 | state: started 106 | daemon_reload: yes 107 | become: yes 108 | 109 | # Hardware config 110 | - name: Power off when button pressed 111 | shell: hostnamectl set-chassis vm 112 | become: yes 113 | -------------------------------------------------------------------------------- /ansible/roles/ubr_ros2/templates/cyclonedds.j2: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /ansible/roles/ubr_ros2/templates/cyclonesysctl.j2: -------------------------------------------------------------------------------- 1 | net.core.rmem_max=2147483647 2 | -------------------------------------------------------------------------------- /ansible/roles/ubr_ros2/templates/robot.j2: -------------------------------------------------------------------------------- 1 | 2 | [Unit] 3 | Description=Start UBR-1 drivers (ROS2) 4 | 5 | [Install] 6 | WantedBy=multi-user.target 7 | 8 | [Service] 9 | Environment="ROS_LOG_DIR=/var/log/ros" 10 | Environment="RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" 11 | Environment="CYCLONEDDS_URI=/etc/ros/{{rosdistro}}/cyclonedds.xml" 12 | ExecStart=/bin/bash -c ". {{workspace_root}}/install/setup.bash && /etc/ros/{{rosdistro}}/robot.launch.py" 13 | StandardOutput=file:/var/log/ros/robot.log 14 | StandardError=file:/var/log/ros/robot.log 15 | User=ros 16 | -------------------------------------------------------------------------------- /ansible/ubr_jazzy.yml: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | - hosts: all 4 | roles: 5 | - ros_user 6 | - ubr_ros2 7 | vars: 8 | rosdistro: iron 9 | workspace_root: /home/ubr/jazzy 10 | -------------------------------------------------------------------------------- /ansible/ubr_ros1.yml: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | - hosts: all 4 | roles: 5 | - ros_user 6 | - ubr_ros1 7 | vars: 8 | rosdistro: noetic 9 | workspace_root: /home/ubr/noetic 10 | -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | ARG WORKSPACE=/opt/workspace 2 | 3 | FROM osrf/ros:humble-desktop 4 | 5 | # install build tools and cyclonedds 6 | RUN apt-get update && apt-get install -q -y --no-install-recommends \ 7 | python3-colcon-common-extensions \ 8 | git-core \ 9 | ros-humble-rmw-cyclonedds-cpp \ 10 | && rm -rf /var/lib/apt/lists/* 11 | 12 | # get ubr code 13 | ARG WORKSPACE 14 | WORKDIR $WORKSPACE/src 15 | RUN git clone https://github.com/mikeferguson/ubr_reloaded.git \ 16 | && touch ubr_reloaded/ubr1_bringup/COLCON_IGNORE \ 17 | && touch ubr_reloaded/ubr1_calibration/COLCON_IGNORE \ 18 | && touch ubr_reloaded/ubr1_gazebo/COLCON_IGNORE \ 19 | && touch ubr_reloaded/ubr1_moveit/COLCON_IGNORE \ 20 | && touch ubr_reloaded/ubr1_navigation/COLCON_IGNORE \ 21 | && touch ubr_reloaded/ubr_msgs/COLCON_IGNORE \ 22 | && touch ubr_reloaded/ubr_teleop/COLCON_IGNORE 23 | 24 | # install dependencies 25 | ARG WORKSPACE 26 | WORKDIR $WORKSPACE 27 | RUN . /opt/ros/$ROS_DISTRO/setup.sh \ 28 | && apt-get update && rosdep install -q -y \ 29 | --from-paths src \ 30 | --ignore-src \ 31 | && rm -rf /var/lib/apt/lists/* 32 | 33 | # build ubr code 34 | ARG WORKSPACE 35 | WORKDIR $WORKSPACE 36 | RUN . /opt/ros/$ROS_DISTRO/setup.sh \ 37 | && colcon build 38 | 39 | # setup entrypoint 40 | COPY ./ros_entrypoint.sh / 41 | 42 | ENTRYPOINT ["/ros_entrypoint.sh"] 43 | CMD ["bash"] 44 | -------------------------------------------------------------------------------- /docker/README.md: -------------------------------------------------------------------------------- 1 | # Customized docker image 2 | 3 | This is a customized docker image that builds the ubr1_description 4 | package (so we can see the robot model) and provides a way to 5 | access the standard rviz configurations. 6 | 7 | sudo apt-get install docker python3-rocker 8 | sudo docker build --tag ubr:main . 9 | sudo rocker --net=host --x11 ubr:main rviz2 10 | -------------------------------------------------------------------------------- /docker/ros_entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | # setup ros2 environment 5 | source "/opt/workspace/install/setup.bash" 6 | export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp 7 | exec "$@" 8 | 9 | -------------------------------------------------------------------------------- /ubr1_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ubr1_bringup) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | install( 7 | DIRECTORY launch 8 | DESTINATION share/${PROJECT_NAME}/ 9 | ) 10 | 11 | install( 12 | DIRECTORY config 13 | DESTINATION share/${PROJECT_NAME}/ 14 | ) 15 | 16 | install( 17 | PROGRAMS scripts/controller_reset.py 18 | scripts/profile_base.py 19 | DESTINATION lib/${PROJECT_NAME} 20 | ) 21 | 22 | if(BUILD_TESTING) 23 | find_package(ament_lint_auto REQUIRED) 24 | ament_lint_auto_find_test_dependencies() 25 | endif() 26 | 27 | ament_package() 28 | -------------------------------------------------------------------------------- /ubr1_bringup/config/default_controllers.yaml: -------------------------------------------------------------------------------- 1 | ubr_driver: 2 | ros__parameters: 3 | default_controllers: 4 | - "arm_controller.cartesian_pose" 5 | - "arm_controller.cartesian_twist" 6 | - "arm_controller.cartesian_wrench" 7 | - "arm_controller.follow_joint_trajectory" 8 | - "arm_controller.gravity_compensation" 9 | - "arm_with_torso_controller.follow_joint_trajectory" 10 | - "base_controller" 11 | - "head_controller.follow_joint_trajectory" 12 | - "head_controller.point_head" 13 | - "torso_controller.follow_joint_trajectory" 14 | arm_controller: 15 | cartesian_pose: 16 | type: "robot_controllers/CartesianPoseController" 17 | fb_trans: 18 | p: 0.75 19 | d: 0.0 20 | i: 0.02 21 | i_clamp: 0.1 22 | fb_rot: 23 | p: 0.25 24 | d: 0.0 25 | i: 0.02 26 | i_clamp: 0.1 27 | cartesian_twist: 28 | type: "robot_controllers/CartesianTwistController" 29 | cartesian_wrench: 30 | type: "robot_controllers/CartesianWrenchController" 31 | gravity_compensation: 32 | type: "robot_controllers/GravityCompensation" 33 | root: "torso_lift_link" 34 | tip: "gripper_link" 35 | autostart: true 36 | follow_joint_trajectory: 37 | type: "robot_controllers/FollowJointTrajectoryController" 38 | joints: 39 | - shoulder_pan_joint 40 | - shoulder_lift_joint 41 | - upperarm_roll_joint 42 | - elbow_flex_joint 43 | - forearm_roll_joint 44 | - wrist_flex_joint 45 | - wrist_roll_joint 46 | arm_with_torso_controller: 47 | follow_joint_trajectory: 48 | type: "robot_controllers/FollowJointTrajectoryController" 49 | joints: 50 | - torso_lift_joint 51 | - shoulder_pan_joint 52 | - shoulder_lift_joint 53 | - upperarm_roll_joint 54 | - elbow_flex_joint 55 | - forearm_roll_joint 56 | - wrist_flex_joint 57 | - wrist_roll_joint 58 | head_controller: 59 | follow_joint_trajectory: 60 | type: "robot_controllers/FollowJointTrajectoryController" 61 | joints: 62 | - head_pan_joint 63 | - head_tilt_joint 64 | point_head: 65 | type: "robot_controllers/PointHeadController" 66 | torso_controller: 67 | follow_joint_trajectory: 68 | type: "robot_controllers/FollowJointTrajectoryController" 69 | joints: 70 | - torso_lift_joint 71 | base_controller: 72 | type: "robot_controllers/DiffDriveBaseController" 73 | max_velocity_x: 1.0 74 | max_acceleration_x: 0.75 75 | # hold position 76 | moving_threshold: -0.01 77 | rotating_threshold: -0.01 78 | # autostart to get odom 79 | autostart: true 80 | # use laser to only slowly collide with things 81 | laser_safety_dist: 1.0 82 | # name of joints 83 | l_wheel_joints: 84 | - "base_l_wheel_joint" 85 | r_wheel_joints: 86 | - "base_r_wheel_joint" 87 | # This parameter is now conditionally set in launch file 88 | # based on whether TF will be published by fuse or driver 89 | # publish_tf: false 90 | -------------------------------------------------------------------------------- /ubr1_bringup/config/fixed_lag_smoother.yaml: -------------------------------------------------------------------------------- 1 | odom_fusion_node: 2 | ros__parameters: 3 | # Fixed-lag smoother configuration 4 | optimization_frequency: 20.0 5 | transaction_timeout: 0.01 6 | lag_duration: 0.5 7 | 8 | motion_models: 9 | unicycle_motion_model: 10 | type: fuse_models::Unicycle2D 11 | 12 | unicycle_motion_model: 13 | # x y yaw vx vy vyaw ax ay 14 | process_noise_diagonal: [0.100, 0.100, 0.100, 0.100, 0.100, 0.100, 0.1, 0.1] 15 | 16 | sensor_models: 17 | initial_localization_sensor: 18 | type: fuse_models::Unicycle2DIgnition 19 | motion_models: [unicycle_motion_model] 20 | ignition: true 21 | odometry_sensor: 22 | type: fuse_models::Odometry2D 23 | motion_models: [unicycle_motion_model] 24 | imu_sensor: 25 | type: fuse_models::Imu2D 26 | motion_models: [unicycle_motion_model] 27 | 28 | initial_localization_sensor: 29 | publish_on_startup: true 30 | # x y yaw vx vy vyaw ax ay 31 | initial_state: [0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] 32 | initial_sigma: [0.100, 0.100, 0.100, 0.100, 0.100, 0.100, 0.100, 0.100] 33 | 34 | odometry_sensor: 35 | topic: 'base_controller/odom' 36 | twist_target_frame: 'base_link' 37 | linear_velocity_dimensions: ['x', 'y'] 38 | angular_velocity_dimensions: ['yaw'] 39 | 40 | imu_sensor: 41 | topic: 'imu' 42 | twist_target_frame: 'base_link' 43 | angular_velocity_dimensions: ['yaw'] 44 | 45 | publishers: 46 | filtered_publisher: 47 | type: fuse_models::Odometry2DPublisher 48 | 49 | filtered_publisher: 50 | topic: 'odom' 51 | base_link_frame_id: 'base_link' 52 | odom_frame_id: 'odom' 53 | map_frame_id: 'map' 54 | world_frame_id: 'odom' 55 | publish_tf: true 56 | publish_frequency: 10.0 57 | -------------------------------------------------------------------------------- /ubr1_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ubr1_bringup 5 | 0.3.0 6 | 7 | Launch Files for UBR-1. 8 | 9 | Michael Ferguson 10 | Michael Ferguson 11 | BSD 12 | http://ros.org/wiki/ubr1_bringup 13 | 14 | ament_cmake 15 | 16 | depth_image_proc 17 | image_proc 18 | fuse 19 | joy 20 | joy_linux 21 | openni2_camera 22 | ubr_teleop 23 | ubr1_description 24 | urg_node 25 | 26 | ament_lint_auto 27 | ament_lint_common 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /ubr1_bringup/scripts/controller_reset.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2020-2024, Michael Ferguson 4 | # Copyright (c) 2015, Fetch Robotics Inc. 5 | # All rights reserved. 6 | # 7 | # Software License Agreement (BSD License 2.0) 8 | # 9 | # Redistribution and use in source and binary forms, with or without 10 | # modification, are permitted provided that the following conditions 11 | # are met: 12 | # 13 | # * Redistributions of source code must retain the above copyright 14 | # notice, this list of conditions and the following disclaimer. 15 | # * Redistributions in binary form must reproduce the above 16 | # copyright notice, this list of conditions and the following 17 | # disclaimer in the documentation and/or other materials provided 18 | # with the distribution. 19 | # * Neither the name of the copyright holder nor the names of its 20 | # contributors may be used to endorse or promote products derived 21 | # from this software without specific prior written permission. 22 | # 23 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | # POSSIBILITY OF SUCH DAMAGE. 35 | 36 | from control_msgs.action import GripperCommand 37 | import rclpy 38 | from rclpy.action import ActionClient 39 | from rclpy.duration import Duration 40 | from rclpy.node import Node 41 | from robot_controllers_msgs.msg import ControllerState 42 | from robot_controllers_msgs.srv import QueryControllerStates 43 | from sensor_msgs.msg import Joy 44 | 45 | 46 | class ControllerResetTeleop(Node): 47 | 48 | def __init__(self): 49 | super().__init__('controller_reset') 50 | 51 | # ROS Service connection to controller state query 52 | self.client = self.create_client(QueryControllerStates, 'query_controller_states') 53 | while not self.client.wait_for_service(timeout_sec=1.0): 54 | print('query_controller_states not available, waiting again...') 55 | 56 | # Connection gripper controller 57 | self.gripper_client = ActionClient(self, GripperCommand, 'gripper_controller/command') 58 | while not self.gripper_client.wait_for_server(timeout_sec=1.0): 59 | print('gripper_controller/command not available, waiting again...') 60 | 61 | self.start = [] 62 | self.start.append('arm_controller.gravity_compensation') 63 | 64 | self.stop = [] 65 | self.stop.append('arm_controller.follow_joint_trajectory') 66 | self.stop.append('arm_with_torso_controller.follow_joint_trajectory') 67 | self.stop.append('torso_controller.follow_joint_trajectory') 68 | self.stop.append('head_controller.follow_joint_trajectory') 69 | self.stop.append('head_controller.point_head') 70 | 71 | button_param = self.declare_parameter('reset_axis', 7) 72 | self.reset_button = button_param.get_parameter_value().integer_value 73 | value_param = self.declare_parameter('reset_value', 1.0) 74 | self.reset_value = value_param.get_parameter_value().double_value 75 | 76 | self.pressed = False 77 | self.pressed_last = None 78 | 79 | self.joy_sub = self.create_subscription(Joy, 'joy', self.joy_callback, 10) 80 | 81 | def joy_callback(self, msg): 82 | try: 83 | if msg.axes[self.reset_button] == self.reset_value: 84 | if not self.pressed: 85 | self.pressed_last = self.get_clock().now() 86 | self.pressed = True 87 | elif (self.pressed_last and 88 | self.get_clock().now() > self.pressed_last + Duration(seconds=1)): 89 | self.reset() 90 | self.pressed_last = None 91 | else: 92 | self.pressed = False 93 | except KeyError: 94 | self.get_logger().warn('reset_button is out of range') 95 | 96 | def reset(self): 97 | # Reset controllers 98 | req = QueryControllerStates.Request() 99 | 100 | for controller in self.start: 101 | state = ControllerState() 102 | state.name = controller 103 | state.state = state.RUNNING 104 | req.updates.append(state) 105 | 106 | for controller in self.stop: 107 | state = ControllerState() 108 | state.name = controller 109 | state.state = state.STOPPED 110 | req.updates.append(state) 111 | 112 | self.future = self.client.call_async(req) 113 | 114 | # Disable gripper torque 115 | goal = GripperCommand.Goal() 116 | goal.command.max_effort = -1.0 117 | self.gripper_client.send_goal_async(goal) 118 | 119 | 120 | if __name__ == '__main__': 121 | rclpy.init() 122 | 123 | try: 124 | c = ControllerResetTeleop() 125 | rclpy.spin(c) 126 | except KeyboardInterrupt: 127 | pass 128 | -------------------------------------------------------------------------------- /ubr1_calibration/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ubr1_calibration) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | install( 7 | DIRECTORY config 8 | DESTINATION share/${PROJECT_NAME}/ 9 | ) 10 | 11 | install( 12 | DIRECTORY launch 13 | DESTINATION share/${PROJECT_NAME}/ 14 | ) 15 | 16 | install( 17 | PROGRAMS 18 | scripts/calibrate_from_bag 19 | scripts/calibrate_manually 20 | scripts/camera_reconfigure.py 21 | DESTINATION lib/${PROJECT_NAME} 22 | ) 23 | 24 | ament_package() 25 | -------------------------------------------------------------------------------- /ubr1_calibration/README.md: -------------------------------------------------------------------------------- 1 | # UBR-1 Calibration 2 | 3 | This is used to calibrate the UBR-1 robot. You probably aren't looking 4 | to do that since you probably don't have a UBR-1 - but you might be 5 | here to see an example of how to use `robot_calibration` in ROS 2. 6 | 7 | There are several config files of interest here: 8 | 9 | * `config/capture.yaml` - The capture step configuration. 10 | * `config/calibrate.yaml` - The calibrate step configuration. 11 | * `config/calibration_poses.yaml` - The capture poses to use. 12 | These have been manually tweaked to be collision free when 13 | directly interpolating between adjacent poses - so that we 14 | don't have to rely on MoveIt to run planning. The first pose 15 | uses a ground plane finder to align the head camera to the 16 | ground, and then every other pose uses the LED finder. 17 | 18 | There are several entry points: 19 | 20 | * `launch/calibrate.launch.py` - Launch file that I use 99% of the 21 | time to calibrate the robot. 22 | * `launch/calibrate_base.launch.py` - Launch file for calibrating base 23 | track width, base rollout, and IMU gain. 24 | * `scripts/calibrate_from_bag` - If the launch file has already 25 | been run, and I have a bagfile, but want to tweak some aspect of the 26 | calibration step, this is how to run it. 27 | * `scripts/manual_calibration` - This runs robot calibration in 28 | manual mode, where you have to move the robot arm and head yourself. 29 | I find this is generally only useful for developing new feature 30 | finders. This is NOT a launch file, since launch would eat our 31 | input data and we would not be able to hit enter... 32 | 33 | An additional aspect of the calibration of the UBR-1 is that the LED detection 34 | works much better if auto exposure and white balance for the Primesense camera 35 | are turned off. This is done by running the `scripts/camera_reconfigure.py` 36 | script, which calls the `SetParameter` service to disable these features. 37 | 38 | There are additional files that are really maybe not great examples: 39 | 40 | * `config/checkerboard_2d` - this directy contains alternate 41 | capture and calibration configurations for using the `Checkerboard2d` 42 | finder. It performs suboptimally to the 3d version for a Primesense 43 | style camera that is already aligned. This particular configuration 44 | doesn't actually calibrate the robot, instead it simply finds the 45 | checkerboard transform to validate the pipeline. 46 | -------------------------------------------------------------------------------- /ubr1_calibration/config/calibrate.yaml: -------------------------------------------------------------------------------- 1 | robot_calibration: 2 | ros__parameters: 3 | verbose: true 4 | base_link: torso_lift_link 5 | calibration_steps: 6 | - single_calibration_step 7 | single_calibration_step: 8 | models: 9 | - arm 10 | - camera 11 | - ground_camera 12 | arm: 13 | type: chain3d 14 | frame: wrist_roll_link 15 | camera: 16 | type: camera3d 17 | frame: head_camera_rgb_optical_frame 18 | ground_camera: 19 | type: camera3d 20 | param_name: camera # this is the same physical camera as above 21 | frame: head_camera_rgb_optical_frame 22 | free_params: 23 | - shoulder_pan_joint 24 | - shoulder_lift_joint 25 | - upperarm_roll_joint 26 | - elbow_flex_joint 27 | - forearm_roll_joint 28 | - wrist_flex_joint 29 | - wrist_roll_joint 30 | - head_tilt_joint 31 | - camera_fx 32 | - camera_fy 33 | - camera_cx 34 | - camera_cy 35 | - camera_z_offset 36 | - camera_z_scaling 37 | free_frames: 38 | - head_camera_rgb_joint 39 | - head_pan_joint 40 | - checkerboard 41 | head_camera_rgb_joint: 42 | x: true 43 | y: true 44 | z: true 45 | roll: true 46 | pitch: true 47 | yaw: true 48 | head_pan_joint: 49 | x: true 50 | y: true 51 | z: true 52 | roll: true 53 | pitch: true 54 | yaw: true 55 | checkerboard: 56 | x: true 57 | y: false 58 | z: true 59 | roll: false 60 | pitch: true 61 | yaw: false 62 | error_blocks: 63 | - hand_eye 64 | - restrict_camera 65 | hand_eye: 66 | type: chain3d_to_chain3d 67 | model_a: camera 68 | model_b: arm 69 | ground_plane: 70 | type: chain3d_to_plane 71 | model: ground_camera 72 | a: 0.0 73 | b: 0.0 74 | c: 1.0 75 | d: 0.0 76 | scale: 1.0 77 | restrict_camera: 78 | type: outrageous 79 | param: head_camera_rgb_joint 80 | joint_scale: 0.0 81 | position_scale: 0.1 82 | rotation_scale: 0.1 83 | -------------------------------------------------------------------------------- /ubr1_calibration/config/calibrate_base.yaml: -------------------------------------------------------------------------------- 1 | base_calibration_node: 2 | ros__parameters: 3 | verbose: true 4 | calibration_steps: 5 | - slow_rollout_back 6 | - slow_rollout_forward 7 | - slow_spin_left 8 | - slow_spin_right 9 | - fast_spin_left 10 | - fast_spin_right 11 | slow_rollout_back: 12 | type: rollout 13 | velocity: -0.2 14 | distance: -1.0 15 | slow_rollout_forward: 16 | type: rollout 17 | velocity: 0.2 18 | distance: 1.0 19 | slow_spin_left: 20 | type: spin 21 | velocity: 0.5 22 | rotations: 1 23 | slow_spin_right: 24 | type: spin 25 | velocity: -0.5 26 | rotations: 1 27 | fast_spin_left: 28 | type: spin 29 | velocity: 1.5 30 | rotations: 1 31 | fast_spin_right: 32 | type: spin 33 | velocity: -1.5 34 | rotations: 1 35 | -------------------------------------------------------------------------------- /ubr1_calibration/config/camera_in_hand/calibrate.yaml: -------------------------------------------------------------------------------- 1 | robot_calibration: 2 | ros__parameters: 3 | verbose: true 4 | base_link: torso_lift_link 5 | calibration_steps: 6 | - single_calibration_step 7 | single_calibration_step: 8 | models: 9 | - gripper_camera 10 | - fixed 11 | gripper_camera: 12 | type: camera3d 13 | frame: gripper_camera_rgb_optical_frame 14 | # We will determine the checkerboard pose in the torso_lift_link 15 | fixed: 16 | type: chain3d 17 | frame: torso_lift_link 18 | free_frames: 19 | - gripper_camera_rgb_joint 20 | - checkerboard 21 | gripper_camera_rgb_joint: 22 | x: true 23 | y: true 24 | z: true 25 | roll: true 26 | pitch: true 27 | yaw: true 28 | checkerboard: 29 | x: true 30 | y: true 31 | z: true 32 | roll: true 33 | pitch: true 34 | yaw: true 35 | error_blocks: 36 | - gripper_camera_to_checkerboard 37 | gripper_camera_to_checkerboard: 38 | type: chain3d_to_chain3d 39 | model_a: gripper_camera 40 | model_b: fixed 41 | -------------------------------------------------------------------------------- /ubr1_calibration/config/camera_in_hand/capture.yaml: -------------------------------------------------------------------------------- 1 | robot_calibration: 2 | ros__parameters: 3 | chains: 4 | - arm 5 | arm: 6 | topic: /arm_controller/follow_joint_trajectory 7 | joints: 8 | - shoulder_pan_joint 9 | - shoulder_lift_joint 10 | - upperarm_roll_joint 11 | - elbow_flex_joint 12 | - forearm_roll_joint 13 | - wrist_flex_joint 14 | - wrist_roll_joint 15 | features: 16 | - checkerboard_finder 17 | checkerboard_finder: 18 | type: robot_calibration::CheckerboardFinder 19 | topic: /gripper_camera/depth_registered/points 20 | camera_info_topic: /gripper_camera/depth_registered/camera_info 21 | camera_sensor_name: gripper_camera # This is actually the camera on the arm (it moves) 22 | chain_sensor_name: fixed # The checkerboard is fixed 23 | debug: false 24 | points_x: 6 25 | points_y: 7 26 | size: 0.02635 27 | -------------------------------------------------------------------------------- /ubr1_calibration/config/capture.yaml: -------------------------------------------------------------------------------- 1 | robot_calibration: 2 | ros__parameters: 3 | chains: 4 | - arm 5 | - head 6 | arm: 7 | topic: /arm_controller/follow_joint_trajectory 8 | joints: 9 | - shoulder_pan_joint 10 | - shoulder_lift_joint 11 | - upperarm_roll_joint 12 | - elbow_flex_joint 13 | - forearm_roll_joint 14 | - wrist_flex_joint 15 | - wrist_roll_joint 16 | #planning_group: arm TODO: revert when moveit is setup 17 | head: 18 | topic: /head_controller/follow_joint_trajectory 19 | joints: 20 | - head_pan_joint 21 | - head_tilt_joint 22 | features: 23 | - led_finder 24 | - ground_plane_finder 25 | led_finder: 26 | type: robot_calibration::LedFinder 27 | action: /gripper_controller/led_command 28 | topic: /head_camera/depth_registered/points 29 | camera_info_topic: /head_camera/depth_registered/camera_info 30 | camera_sensor_name: camera 31 | chain_sensor_name: arm 32 | threshold: 500.0 33 | max_iterations: 25 34 | debug: false 35 | gripper_led_frame: wrist_roll_link 36 | leds: 37 | - led1 38 | - led2 39 | - led3 40 | - led4 41 | led1: 42 | code: 1 43 | topic: led1 44 | x: 0.0621 45 | y: 0.023 46 | z: 0.026875 47 | led2: 48 | code: 2 49 | topic: led2 50 | x: 0.027 51 | y: 0.023 52 | z: 0.026875 53 | led3: 54 | code: 4 55 | topic: led3 56 | x: 0.061 57 | y: -0.023 58 | z: 0.026875 59 | led4: 60 | code: 8 61 | topic: led4 62 | x: 0.027 63 | y: -0.023 64 | z: 0.026875 65 | gripper_plane_finder: 66 | type: robot_calibration::PlaneFinder 67 | topic: /head_camera/depth_registered/points 68 | camera_info_topic: /head_camera/depth_registered/camera_info 69 | camera_sensor_name: gripper_plane_camera 70 | transform_frame: gripper_link 71 | min_x: 0.02 72 | max_x: 0.1 73 | min_y: -0.15 74 | max_y: 0.15 75 | min_z: 0.0 76 | max_z: 0.15 77 | tolerance: 0.005 78 | points_max: 100 79 | debug: false 80 | ground_plane_finder: 81 | type: robot_calibration::PlaneFinder 82 | topic: /head_camera/depth_registered/points 83 | camera_info_topic: /head_camera/depth_registered/camera_info 84 | camera_sensor_name: ground_camera 85 | points_max: 60 86 | debug: false 87 | -------------------------------------------------------------------------------- /ubr1_calibration/config/checkerboard_2d/calibrate.yaml: -------------------------------------------------------------------------------- 1 | # This is a sample for testing the 2d checkerboard finder 2 | # No part of the robot is actually being calibrated, 3 | # It will simply find the checkerboard pose in the hand 4 | robot_calibration: 5 | ros__parameters: 6 | verbose: true 7 | base_link: torso_lift_link 8 | calibration_steps: 9 | - single_calibration_step 10 | single_calibration_step: 11 | models: 12 | - arm 13 | - camera 14 | arm: 15 | type: chain3d 16 | frame: wrist_roll_link 17 | camera: 18 | type: camera2d 19 | frame: head_camera_rgb_optical_frame 20 | free_frames: 21 | - checkerboard 22 | checkerboard: 23 | x: true 24 | y: false 25 | z: true 26 | roll: true 27 | pitch: true 28 | yaw: true 29 | error_blocks: 30 | - hand_eye 31 | hand_eye: 32 | type: chain3d_to_camera2d 33 | model_2d: camera 34 | model_3d: arm 35 | -------------------------------------------------------------------------------- /ubr1_calibration/config/checkerboard_2d/capture.yaml: -------------------------------------------------------------------------------- 1 | robot_calibration: 2 | ros__parameters: 3 | chains: 4 | - arm 5 | - head 6 | arm: 7 | topic: /arm_controller/follow_joint_trajectory 8 | joints: 9 | - shoulder_pan_joint 10 | - shoulder_lift_joint 11 | - upperarm_roll_joint 12 | - elbow_flex_joint 13 | - forearm_roll_joint 14 | - wrist_flex_joint 15 | - wrist_roll_joint 16 | #planning_group: arm TODO: revert when moveit is setup 17 | head: 18 | topic: /head_controller/follow_joint_trajectory 19 | joints: 20 | - head_pan_joint 21 | - head_tilt_joint 22 | features: 23 | - checkerboard_finder 24 | checkerboard_finder: 25 | type: robot_calibration::CheckerboardFinder2d 26 | topic: /head_camera/rgb/image_raw 27 | camera_sensor_name: camera 28 | chain_sensor_name: arm 29 | debug: false 30 | points_x: 6 31 | points_y: 7 32 | size: 0.02635 33 | -------------------------------------------------------------------------------- /ubr1_calibration/launch/calibrate.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2020-2024, Michael Ferguson 4 | # All rights reserved. 5 | # 6 | # Software License Agreement (BSD License 2.0) 7 | # 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions 10 | # are met: 11 | # 12 | # * Redistributions of source code must retain the above copyright 13 | # notice, this list of conditions and the following disclaimer. 14 | # * Redistributions in binary form must reproduce the above 15 | # copyright notice, this list of conditions and the following 16 | # disclaimer in the documentation and/or other materials provided 17 | # with the distribution. 18 | # * Neither the name of the copyright holder nor the names of its 19 | # contributors may be used to endorse or promote products derived 20 | # from this software without specific prior written permission. 21 | # 22 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | # POSSIBILITY OF SUCH DAMAGE. 34 | 35 | import os 36 | import sys 37 | 38 | from ament_index_python.packages import get_package_share_directory 39 | 40 | from launch import LaunchDescription 41 | from launch.actions import ExecuteProcess 42 | from launch_ros.actions import Node 43 | 44 | 45 | def generate_launch_description(): 46 | # Load the capture config 47 | capture_config = os.path.join( 48 | get_package_share_directory('ubr1_calibration'), 49 | 'config', 50 | 'capture.yaml' 51 | ) 52 | 53 | # Load the calibration config 54 | calibration_config = os.path.join( 55 | get_package_share_directory('ubr1_calibration'), 56 | 'config', 57 | 'calibrate.yaml' 58 | ) 59 | 60 | # Load the calibration poses YAML 61 | calibration_poses = os.path.join( 62 | get_package_share_directory('ubr1_calibration'), 63 | 'config', 64 | 'calibration_poses.yaml' 65 | ) 66 | 67 | # Make a directory for bagfiles to be located 68 | try: 69 | os.mkdir("/tmp/ubr1_calibration") 70 | except FileExistsError: 71 | pass 72 | 73 | return LaunchDescription([ 74 | # Turn off auto exposure on camera for better results 75 | Node( 76 | name='camera_reconfigure', 77 | package='ubr1_calibration', 78 | executable='camera_reconfigure.py', 79 | arguments=['--disable'], 80 | output='screen' 81 | ), 82 | # Calibration 83 | Node( 84 | name='robot_calibration', 85 | package='robot_calibration', 86 | executable='calibrate', 87 | arguments=[calibration_poses], 88 | parameters=[capture_config, 89 | calibration_config], 90 | output='screen', 91 | ), 92 | # Record bagfile for debugging 93 | ExecuteProcess( 94 | cmd=["ros2", "bag", "record", "/calibration_data", "/robot_description"], 95 | cwd="/tmp/ubr1_calibration" 96 | ) 97 | ]) 98 | -------------------------------------------------------------------------------- /ubr1_calibration/launch/calibrate_base.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020-2024, Michael Ferguson 2 | # All rights reserved. 3 | # 4 | # Software License Agreement (BSD License 2.0) 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of the copyright holder nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | import os 34 | import sys 35 | 36 | from ament_index_python.packages import get_package_share_directory 37 | 38 | from launch import LaunchDescription 39 | from launch_ros.actions import Node 40 | 41 | 42 | def generate_launch_description(): 43 | calibration_config = os.path.join( 44 | get_package_share_directory('ubr1_calibration'), 45 | 'config', 46 | 'calibrate_base.yaml' 47 | ) 48 | 49 | return LaunchDescription([ 50 | Node( 51 | name='base_calibration_node', 52 | package='robot_calibration', 53 | executable='base_calibration_node', 54 | parameters=[calibration_config], 55 | remappings=[('odom', 'base_controller/odom'), 56 | ('imu', 'imu/filtered')], 57 | output='screen', 58 | ), 59 | ]) 60 | -------------------------------------------------------------------------------- /ubr1_calibration/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ubr1_calibration 5 | 0.3.0 6 | 7 | Launch and configuration files for calibrating UBR-1 using the 'robot_calibration' package. 8 | 9 | 10 | Michael Ferguson 11 | Michael Ferguson 12 | 13 | Apache2 14 | 15 | ament_cmake 16 | 17 | robot_calibration 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /ubr1_calibration/scripts/calibrate_from_bag: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | ros2 run robot_calibration calibrate --from-bag $1 --ros-args --params-file ../config/calibrate.yaml 4 | -------------------------------------------------------------------------------- /ubr1_calibration/scripts/calibrate_manually: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # ros2 launch (from python) doesn't seem to pipe stdin to our node, so manual capture doesn't work 4 | ros2 run robot_calibration calibrate --manual --ros-args --params-file ../config/capture.yaml --params-file ../config/calibrate.yaml 5 | -------------------------------------------------------------------------------- /ubr1_calibration/scripts/camera_in_hand/camera_in_hand_manually: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # ros2 launch (from python) doesn't seem to pipe stdin to our node, so manual capture doesn't work 4 | ros2 run robot_calibration calibrate --manual --ros-args --params-file ../../config/camera_in_hand/capture.yaml --params-file ../../config/camera_in_hand/calibrate.yaml 5 | -------------------------------------------------------------------------------- /ubr1_calibration/scripts/camera_reconfigure.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (C) 2024 Michael Ferguson 4 | # Copyright (C) 2015 Fetch Robotics Inc 5 | # 6 | # Licensed under the Apache License, Version 2.0 (the "License"); 7 | # you may not use this file except in compliance with the License. 8 | # You may obtain a copy of the License at 9 | # 10 | # http://www.apache.org/licenses/LICENSE-2.0 11 | # 12 | # Unless required by applicable law or agreed to in writing, software 13 | # distributed under the License is distributed on an "AS IS" BASIS, 14 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | # See the License for the specific language governing permissions and 16 | # limitations under the License. 17 | 18 | # Author: Michael Ferguson 19 | 20 | import sys 21 | import rclpy 22 | from rclpy.node import Node 23 | from rcl_interfaces.srv import SetParameters 24 | from rcl_interfaces.msg import Parameter, ParameterType 25 | 26 | 27 | class CameraReconfigure(Node): 28 | 29 | def __init__(self): 30 | super().__init__('camera_reconfigure') 31 | self.client = self.create_client(SetParameters, 32 | '/head_camera/driver/set_parameters') 33 | while not self.client.wait_for_service(timeout_sec=10.0): 34 | self.get_logger().info('Waiting for service') 35 | 36 | def enable_auto(self, enable): 37 | exposure = Parameter() 38 | exposure.name = 'auto_exposure' 39 | exposure.value.type = ParameterType.PARAMETER_BOOL 40 | exposure.value.bool_value = enable 41 | 42 | white_balance = Parameter() 43 | white_balance.name = 'auto_white_balance' 44 | white_balance.value.type = ParameterType.PARAMETER_BOOL 45 | white_balance.value.bool_value = enable 46 | 47 | request = SetParameters.Request() 48 | request.parameters = [exposure, white_balance] 49 | 50 | self.future = self.client.call_async(request) 51 | rclpy.spin_until_future_complete(self, self.future) 52 | result = self.future.result() 53 | for result in result.results: 54 | if not result.successful: 55 | self.get_logger().warn('Unable to set parameter') 56 | return 57 | self.get_logger().info('Camera configured') 58 | 59 | 60 | if __name__ == "__main__": 61 | if len(sys.argv) < 2: 62 | print("Usage: camera_reconfigure --enable/disable") 63 | exit(-1) 64 | 65 | rclpy.init() 66 | reconfigure = CameraReconfigure() 67 | 68 | if sys.argv[1] == "--enable": 69 | reconfigure.enable_auto(True) 70 | else: 71 | reconfigure.enable_auto(False) 72 | -------------------------------------------------------------------------------- /ubr1_calibration/scripts/checkerboard_2d/checkerboard_2d_manually: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # ros2 launch (from python) doesn't seem to pipe stdin to our node, so manual capture doesn't work 4 | ros2 run robot_calibration calibrate --manual --ros-args --params-file ../../config/checkerboard_2d/capture.yaml --params-file ../../config/checkerboard_2d/calibrate.yaml 5 | -------------------------------------------------------------------------------- /ubr1_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(ubr1_demo) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(grasping_msgs REQUIRED) 11 | find_package(moveit_task_constructor_core REQUIRED) 12 | find_package(rclcpp REQUIRED) 13 | find_package(simple_actions REQUIRED) 14 | 15 | add_executable(pick_and_place 16 | src/generate_grasps_from_msg.cpp 17 | src/pick_and_place.cpp 18 | src/pick_place_task.cpp 19 | ) 20 | target_include_directories(pick_and_place PUBLIC 21 | $ 22 | $) 23 | target_compile_features(pick_and_place PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 24 | ament_target_dependencies( 25 | pick_and_place 26 | grasping_msgs 27 | moveit_task_constructor_core 28 | rclcpp 29 | simple_actions 30 | ) 31 | 32 | install(TARGETS pick_and_place 33 | DESTINATION lib/${PROJECT_NAME}) 34 | 35 | install( 36 | DIRECTORY launch 37 | DESTINATION share/${PROJECT_NAME}/ 38 | ) 39 | 40 | ament_package() 41 | -------------------------------------------------------------------------------- /ubr1_demo/include/ubr1_demo/generate_grasps_from_msg.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Hamburg University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Bielefeld University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #pragma once 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | namespace moveit { 42 | namespace task_constructor { 43 | namespace stages { 44 | 45 | class GenerateGraspsFromMsg : public MonitoringGenerator 46 | { 47 | public: 48 | GenerateGraspsFromMsg(const std::string& name = "generate grasp poses from msg"); 49 | 50 | void reset() override; 51 | bool canCompute() const override; 52 | void compute() override; 53 | 54 | void setGrasps(const std::vector grasps) { setProperty("grasps", grasps); } 55 | 56 | protected: 57 | void onNewSolution(const SolutionBase& s) override; 58 | ordered upstream_solutions_; 59 | }; 60 | } // namespace stages 61 | } // namespace task_constructor 62 | } // namespace moveit 63 | -------------------------------------------------------------------------------- /ubr1_demo/include/ubr1_demo/pick_place_task.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2024 Michael Ferguson 5 | * Copyright (c) 2019 PickNik LLC. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of the copyright holder nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *********************************************************************/ 33 | 34 | #ifndef UBR1_DEMO_PICK_PLACE_TASK_HPP 35 | #define UBR1_DEMO_PICK_PLACE_TASK_HPP 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | class PickPlaceTask 43 | { 44 | public: 45 | PickPlaceTask(const std::string& task_name); 46 | ~PickPlaceTask() = default; 47 | 48 | bool configure( 49 | const rclcpp::Node::SharedPtr& node, 50 | const grasping_msgs::msg::Object & object, 51 | const std::vector & grasps, 52 | const std::string & arm_group_name, 53 | const std::string & gripper_group_name, 54 | const std::string & eef_frame); 55 | 56 | bool plan(const std::size_t max_solutions); 57 | 58 | bool execute(); 59 | 60 | private: 61 | std::string task_name_; 62 | moveit::task_constructor::TaskPtr task_; 63 | }; 64 | 65 | #endif // UBR1_DEMO_PICK_PLACE_TASK_HPP 66 | -------------------------------------------------------------------------------- /ubr1_demo/launch/pick_place.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from moveit_configs_utils import MoveItConfigsBuilder 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder("ubr1", package_name="ubr1_moveit").to_dict() 7 | 8 | # MTC Demo node 9 | pick_place_demo = Node( 10 | package="ubr1_demo", 11 | executable="pick_and_place", 12 | output="screen", 13 | parameters=[ 14 | moveit_config, 15 | ], 16 | ) 17 | 18 | return LaunchDescription([pick_place_demo]) 19 | -------------------------------------------------------------------------------- /ubr1_demo/launch/simple_grasping.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2020-2024, Michael Ferguson 4 | # All rights reserved. 5 | # 6 | # Software License Agreement (BSD License 2.0) 7 | # 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions 10 | # are met: 11 | # 12 | # * Redistributions of source code must retain the above copyright 13 | # notice, this list of conditions and the following disclaimer. 14 | # * Redistributions in binary form must reproduce the above 15 | # copyright notice, this list of conditions and the following 16 | # disclaimer in the documentation and/or other materials provided 17 | # with the distribution. 18 | # * Neither the name of the copyright holder nor the names of its 19 | # contributors may be used to endorse or promote products derived 20 | # from this software without specific prior written permission. 21 | # 22 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | # POSSIBILITY OF SUCH DAMAGE. 34 | 35 | import os 36 | import sys 37 | 38 | from ament_index_python.packages import get_package_share_directory 39 | 40 | from launch import LaunchDescription, LaunchService 41 | from launch_ros.actions import Node 42 | 43 | import yaml 44 | 45 | 46 | def generate_launch_description(): 47 | 48 | return LaunchDescription([ 49 | # Drivers 50 | Node( 51 | name='basic_grasping_perception_node', 52 | package='simple_grasping', 53 | executable='basic_grasping_perception_node', 54 | parameters=[{ 55 | # No longer needed with CycloneDDS working on 24.04 56 | #'qos_overrides': { 57 | # '/head_camera/depth_registered/points': { 58 | # 'subscription': { 59 | # 'reliability': 'reliable', 60 | # } 61 | # } 62 | #}, 63 | 'debug_topics': True, 64 | 'gripper/tool_to_planning_frame': 0.120, 65 | 'gripper/max_opening': 0.09, 66 | }], 67 | ) 68 | ]) 69 | 70 | 71 | def main(argv=sys.argv[1:]): 72 | ld = generate_launch_description() 73 | ls = LaunchService(argv=argv) 74 | ls.include_launch_description(ld) 75 | return ls.run() 76 | 77 | 78 | if __name__ == '__main__': 79 | main() 80 | -------------------------------------------------------------------------------- /ubr1_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ubr1_demo 5 | 0.3.0 6 | Demos with the UBR-1 7 | Michael Ferguson 8 | BSD 9 | 10 | ament_cmake 11 | 12 | grasping_msgs 13 | moveit_task_constructor_core 14 | rclcpp 15 | simple_actions 16 | 17 | simple_grasping 18 | 19 | ament_lint_auto 20 | ament_lint_common 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /ubr1_demo/src/generate_grasps_from_msg.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2024, Michael Ferguson 5 | * Copyright (c) 2017, Bielefeld + Hamburg University 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Bielefeld University nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | namespace moveit { 44 | namespace task_constructor { 45 | namespace stages { 46 | 47 | static const rclcpp::Logger LOGGER = rclcpp::get_logger("GenerateGraspsFromMsg"); 48 | 49 | GenerateGraspsFromMsg::GenerateGraspsFromMsg(const std::string& name) : MonitoringGenerator(name) 50 | { 51 | setCostTerm(std::make_unique(0.0)); 52 | 53 | auto& p = properties(); 54 | p.declare>("grasps", "grasps to pass on in spawned states"); 55 | } 56 | 57 | void GenerateGraspsFromMsg::reset() 58 | { 59 | upstream_solutions_.clear(); 60 | MonitoringGenerator::reset(); 61 | } 62 | 63 | void GenerateGraspsFromMsg::onNewSolution(const SolutionBase& s) 64 | { 65 | // It's safe to store a pointer to this solution, as the generating stage stores it 66 | upstream_solutions_.push(&s); 67 | } 68 | 69 | bool GenerateGraspsFromMsg::canCompute() const 70 | { 71 | return !upstream_solutions_.empty(); 72 | } 73 | 74 | void GenerateGraspsFromMsg::compute() 75 | { 76 | if (upstream_solutions_.empty()) 77 | return; 78 | 79 | const SolutionBase& s = *upstream_solutions_.pop(); 80 | planning_scene::PlanningSceneConstPtr scene = s.end()->scene()->diff(); 81 | 82 | std::vector grasps = properties().get>("grasps"); 83 | for (auto grasp : grasps) 84 | { 85 | InterfaceState state(scene); 86 | forwardProperties(*s.end(), state); // forward registered properties from received solution 87 | 88 | state.properties().set("target_pose", grasp.grasp_pose); 89 | 90 | SubTrajectory trajectory; 91 | trajectory.setCost(0.0); 92 | 93 | rviz_marker_tools::appendFrame(trajectory.markers(), grasp.grasp_pose, 0.1, "pose frame"); 94 | spawn(std::move(state), std::move(trajectory)); 95 | } 96 | 97 | } 98 | } // namespace stages 99 | } // namespace task_constructor 100 | } // namespace moveit 101 | -------------------------------------------------------------------------------- /ubr1_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ubr1_description) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | install( 7 | DIRECTORY robots 8 | DESTINATION share/${PROJECT_NAME}/ 9 | ) 10 | 11 | install( 12 | DIRECTORY meshes 13 | DESTINATION share/${PROJECT_NAME}/ 14 | ) 15 | 16 | ament_package() 17 | -------------------------------------------------------------------------------- /ubr1_description/meshes/base_laser_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/base_laser_link.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/base_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/base_link.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/base_link_col.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/base_link_col.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/bellows_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/bellows_link.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/bellows_link_col.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/bellows_link_col.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/elbow_flex_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/elbow_flex_link.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/estop_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/estop_link.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/fixed_torso_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/fixed_torso_link.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/forearm_roll_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/forearm_roll_link.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/forearm_roll_link_col.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/forearm_roll_link_col.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/gripper_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/gripper_link.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/gripper_link_col.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/gripper_link_col.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/head_camera_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/head_camera_link.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/head_pan_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/head_pan_link.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/head_tilt_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/head_tilt_link.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/left_gripper_finger_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/left_gripper_finger_link.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/left_wheel_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/left_wheel_link.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/right_gripper_finger_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/right_gripper_finger_link.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/right_wheel_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/right_wheel_link.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/shoulder_lift_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/shoulder_lift_link.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/shoulder_lift_link_col.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/shoulder_lift_link_col.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/shoulder_pan_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/shoulder_pan_link.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/shoulder_pan_link_col.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/shoulder_pan_link_col.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/torso_lift_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/torso_lift_link.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/torso_lift_link_col.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/torso_lift_link_col.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/upperarm_roll_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/upperarm_roll_link.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/upperarm_roll_link_col.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/upperarm_roll_link_col.STL -------------------------------------------------------------------------------- /ubr1_description/meshes/wrist_flex_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_description/meshes/wrist_flex_link.STL -------------------------------------------------------------------------------- /ubr1_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ubr1_description 5 | 0.3.0 6 | 7 | URDF for UBR-1. 8 | 9 | Michael Ferguson 10 | Melonee Wise 11 | Michael Ferguson 12 | All Rights Reserved 13 | http://ros.org/wiki/ubr1_description 14 | 15 | ament_cmake 16 | 17 | urdf 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /ubr1_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(ubr1_gazebo) 3 | 4 | find_package(gazebo REQUIRED) 5 | 6 | find_package(catkin 7 | REQUIRED 8 | control_toolbox 9 | gazebo_ros 10 | robot_controllers 11 | robot_controllers_interface 12 | ) 13 | 14 | link_directories( 15 | ${GAZEBO_LIBRARY_DIRS} 16 | ) 17 | 18 | include_directories( 19 | include 20 | ${catkin_INCLUDE_DIRS} 21 | ${GAZEBO_INCLUDE_DIRS} 22 | ) 23 | 24 | catkin_package( 25 | CATKIN_DEPENDS 26 | control_toolbox 27 | gazebo_ros 28 | robot_controllers 29 | robot_controllers_interface 30 | DEPENDS 31 | gazebo 32 | ) 33 | 34 | add_library(UBR1GazeboPlugin src/ubr1_gazebo_plugin.cpp) 35 | target_link_libraries(UBR1GazeboPlugin 36 | ${Boost_LIBRARIES} 37 | ${catkin_LIBRARIES} 38 | ${GAZEBO_LIBRARIES} 39 | ) 40 | 41 | add_library(ubr1_gazebo_controllers 42 | src/simulated_bellows_controller.cpp 43 | src/simulated_gripper_controller.cpp 44 | ) 45 | target_link_libraries(ubr1_gazebo_controllers 46 | ${Boost_LIBRARIES} 47 | ${catkin_LIBRARIES} 48 | ) 49 | 50 | install( 51 | DIRECTORY 52 | config 53 | launch 54 | robots 55 | worlds 56 | models 57 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 58 | ) 59 | 60 | install( 61 | PROGRAMS 62 | scripts/stop_controllers.py 63 | test/pick_up_cube_test.py 64 | test/scripts/generate_grasp_test.py 65 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 66 | ) 67 | 68 | install( 69 | FILES 70 | test/pick_up_cube_trajectories.bag 71 | test/pick_up_cube_test.launch 72 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 73 | ) 74 | 75 | install( 76 | FILES ubr1_gazebo_controllers.xml 77 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 78 | ) 79 | 80 | install( 81 | TARGETS 82 | UBR1GazeboPlugin 83 | ubr1_gazebo_controllers 84 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 85 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 86 | ) 87 | -------------------------------------------------------------------------------- /ubr1_gazebo/COLCON_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_gazebo/COLCON_IGNORE -------------------------------------------------------------------------------- /ubr1_gazebo/config/default_controllers.yaml: -------------------------------------------------------------------------------- 1 | arm_controller: 2 | follow_joint_trajectory: 3 | type: "robot_controllers/FollowJointTrajectoryController" 4 | joints: 5 | - shoulder_pan_joint 6 | - shoulder_lift_joint 7 | - upperarm_roll_joint 8 | - elbow_flex_joint 9 | - forearm_roll_joint 10 | - wrist_flex_joint 11 | - wrist_roll_joint 12 | gravity_compensation: 13 | type: "robot_controllers/GravityCompensation" 14 | root: "torso_lift_link" 15 | tip: "gripper_link" 16 | autostart: true 17 | 18 | arm_with_torso_controller: 19 | follow_joint_trajectory: 20 | type: "robot_controllers/FollowJointTrajectoryController" 21 | joints: 22 | - torso_lift_joint 23 | - shoulder_pan_joint 24 | - shoulder_lift_joint 25 | - upperarm_roll_joint 26 | - elbow_flex_joint 27 | - forearm_roll_joint 28 | - wrist_flex_joint 29 | - wrist_roll_joint 30 | 31 | torso_controller: 32 | follow_joint_trajectory: 33 | type: "robot_controllers/FollowJointTrajectoryController" 34 | joints: 35 | - torso_lift_joint 36 | 37 | head_controller: 38 | point_head: 39 | type: "robot_controllers/PointHeadController" 40 | follow_joint_trajectory: 41 | type: "robot_controllers/FollowJointTrajectoryController" 42 | joints: 43 | - head_pan_joint 44 | - head_tilt_joint 45 | 46 | base_controller: 47 | type: "robot_controllers/DiffDriveBaseController" 48 | max_velocity_x: 1.0 49 | max_acceleration_x: 0.75 50 | autostart: true 51 | # name of joints 52 | l_wheel_joint: base_l_wheel_joint 53 | r_wheel_joint: base_r_wheel_joint 54 | 55 | gripper_controller: 56 | gripper_action: 57 | type: "ubr1_gazebo_controllers/SimulatedGripperController" 58 | 59 | bellows_controller: 60 | type: "ubr1_gazebo_controllers/SimulatedBellowsController" 61 | 62 | gazebo: 63 | default_controllers: 64 | - "arm_controller/follow_joint_trajectory" 65 | - "arm_controller/gravity_compensation" 66 | - "arm_with_torso_controller/follow_joint_trajectory" 67 | - "base_controller" 68 | - "head_controller/follow_joint_trajectory" 69 | - "head_controller/point_head" 70 | - "torso_controller/follow_joint_trajectory" 71 | - "gripper_controller/gripper_action" 72 | - "bellows_controller" 73 | base_l_wheel_joint: 74 | position: 75 | p: 0.0 76 | d: 0.0 77 | i: 0.0 78 | i_clamp: 0.0 79 | velocity: 80 | p: 8.0 81 | d: 0.0 82 | i: 0.5 83 | i_clamp: 6.0 84 | effort_limit: 12.0 85 | base_r_wheel_joint: 86 | position: 87 | p: 0.0 88 | d: 0.0 89 | i: 0.0 90 | i_clamp: 0.0 91 | velocity: 92 | p: 8.0 93 | d: 0.0 94 | i: 0.5 95 | i_clamp: 6.0 96 | effort_limit: 12.0 97 | torso_lift_joint: 98 | position: 99 | p: 3000.0 100 | d: 200.0 101 | i: 1.0 102 | i_clamp: 20.0 103 | velocity: 104 | p: 0.0 105 | d: 0.0 106 | i: 0.0 107 | i_clamp: 0.0 108 | effort_offset: 308.0 109 | bellows_joint: 110 | position: 111 | p: 50.0 112 | d: 0.0 113 | i: 0.0 114 | i_clamp: 0.0 115 | velocity: 116 | p: 1.0 117 | d: 0.0 118 | i: 0.0 119 | i_clamp: 0.0 120 | head_pan_joint: 121 | position: 122 | p: 200.0 123 | d: 0.0 124 | i: 0.5 125 | i_clamp: 0.1 126 | velocity: 127 | p: 0.864 128 | d: 0.0 129 | i: 0.128 130 | i_clamp: 0.32 131 | head_tilt_joint: 132 | position: 133 | p: 200.0 134 | d: 0.0 135 | i: 0.5 136 | i_clamp: 0.1 137 | velocity: 138 | p: 0.7668 139 | d: 0.0 140 | i: 0.1136 141 | i_clamp: 0.284 142 | shoulder_pan_joint: 143 | position: 144 | p: 100.0 145 | d: 10.0 146 | i: 0.0 147 | i_clamp: 0.0 148 | velocity: 149 | p: 1.0 150 | d: 0.1 151 | i: 0.0 152 | i_clamp: 0.0 153 | shoulder_lift_joint: 154 | position: 155 | p: 300.0 156 | d: 12.0 157 | i: 1.0 158 | i_clamp: 12.0 159 | velocity: 160 | p: 1.0 161 | d: 0.1 162 | i: 0.0 163 | i_clamp: 0.0 164 | upperarm_roll_joint: 165 | position: 166 | p: 300.0 167 | d: 30.0 168 | i: 3.49 169 | i_clamp: 8.725 170 | velocity: 171 | p: 0.001 172 | d: 0.0 173 | i: 0.0002 174 | i_clamp: 0.025 175 | elbow_flex_joint: 176 | position: 177 | p: 200.0 178 | d: 0.0 179 | i: 30 180 | i_clamp: 1.3 181 | velocity: 182 | p: 0.001 183 | d: 0.0 184 | i: 0.0002 185 | i_clamp: 0.025 186 | forearm_roll_joint: 187 | position: 188 | p: 100.0 189 | d: 0.0 190 | i: 15 191 | i_clamp: 0.5 192 | velocity: 193 | p: 0.001 194 | d: 0.0 195 | i: 0.0002 196 | i_clamp: 0.0125 197 | effort_limit: 3.166 198 | wrist_flex_joint: 199 | position: 200 | p: 50.0 201 | d: 5.0 202 | i: 1.0 203 | i_clamp: 0.3 204 | velocity: 205 | p: 0.0 206 | d: 0.0 207 | i: 0.0 208 | i_clamp: 0.0 209 | wrist_roll_joint: 210 | position: 211 | p: 30.0 212 | d: 3.0 213 | i: 1.0 214 | i_clamp: 0.3 215 | velocity: 216 | p: 0.005 217 | d: 0.0 218 | i: 0.0005 219 | i_clamp: 0.2 220 | effort_limit: 1.0 221 | left_gripper_joint: 222 | position: 223 | p: 2000.0 224 | d: 0.0 225 | i: 0.0 226 | i_clamp: 0.0 227 | velocity: 228 | p: 0.0 229 | d: 0.0 230 | i: 0.0 231 | i_clamp: 0.0 232 | right_gripper_joint: 233 | position: 234 | p: 2000.0 235 | d: 0.0 236 | i: 0.0 237 | i_clamp: 0.0 238 | velocity: 239 | p: 0.0 240 | d: 0.0 241 | i: 0.0 242 | i_clamp: 0.0 243 | -------------------------------------------------------------------------------- /ubr1_gazebo/include/ubr1_gazebo/simulated_bellows_controller.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Michael Ferguson 5 | * Copyright (c) 2014, Unbounded Robotics Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Unbounded Robotics nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | /* Author: Michael Ferguson */ 37 | 38 | #ifndef UBR1_GAZEBO_CONTROLLERS_SIMULATED_BELLOWS_CONTROLLER_H_ 39 | #define UBR1_GAZEBO_CONTROLLERS_SIMULATED_BELLOWS_CONTROLLER_H_ 40 | 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | namespace ubr1_gazebo_controllers 50 | { 51 | 52 | /** 53 | * \brief World's stupidest controller -- this makes the bellows 54 | * be in the right place. If you ever find yourself reading this 55 | * code... I apologize in advance. 56 | */ 57 | class SimulatedBellowsController : public robot_controllers::Controller 58 | { 59 | public: 60 | SimulatedBellowsController() : initialized_(false) {} 61 | virtual ~SimulatedBellowsController() {} 62 | 63 | /** \brief Initialize parameters, interfaces */ 64 | virtual int init(ros::NodeHandle& nh, robot_controllers::ControllerManager* manager); 65 | 66 | /** \brief Start the controller. */ 67 | virtual bool start(); 68 | 69 | /** \brief Stop the controller. */ 70 | virtual bool stop(bool force); 71 | 72 | /** \brief Reset the controller. */ 73 | virtual bool reset(); 74 | 75 | /** \brief Update controller, called from controller_manager update */ 76 | virtual void update(const ros::Time& now, const ros::Duration& dt); 77 | 78 | virtual std::string getType() 79 | { 80 | return "SimulatedBellowsController"; 81 | } 82 | 83 | /** \brief Get a list of joints this controls. */ 84 | virtual std::vector getCommandedNames(); 85 | virtual std::vector getClaimedNames(); 86 | 87 | private: 88 | bool initialized_; 89 | robot_controllers::JointHandlePtr bellows_; 90 | robot_controllers::JointHandlePtr torso_lift_; 91 | }; 92 | 93 | } // namespace ubr1_gazebo_controllers 94 | 95 | #endif // UBR1_GAZEBO_CONTROLLERS_SIMULATED_BELLOWS_CONTROLLER_H_ 96 | -------------------------------------------------------------------------------- /ubr1_gazebo/include/ubr1_gazebo/simulated_gripper_controller.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Michael Ferguson 5 | * Copyright (c) 2013-2014, Unbounded Robotics Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Unbounded Robotics nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | /* Author: Michael Ferguson */ 37 | 38 | #ifndef UBR1_GAZEBO_CONTROLLERS_SIMULATED_GRIPPER_CONTROLLER_H_ 39 | #define UBR1_GAZEBO_CONTROLLERS_SIMULATED_GRIPPER_CONTROLLER_H_ 40 | 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | #include 52 | 53 | namespace ubr1_gazebo_controllers 54 | { 55 | 56 | /** 57 | * \brief Controller for simulating the gripper 58 | */ 59 | class SimulatedGripperController : public robot_controllers::Controller 60 | { 61 | typedef actionlib::SimpleActionServer server_t; 62 | 63 | public: 64 | SimulatedGripperController() : initialized_(false) {} 65 | virtual ~SimulatedGripperController() {} 66 | 67 | /** \brief Initialize parameters, interfaces */ 68 | virtual int init(ros::NodeHandle& nh, robot_controllers::ControllerManager* manager); 69 | 70 | /** \brief Start the controller. */ 71 | virtual bool start(); 72 | 73 | /** \brief Stop the controller. */ 74 | virtual bool stop(bool force); 75 | 76 | /** \brief Reset the controller. */ 77 | virtual bool reset(); 78 | 79 | /** \brief Update controller, called from controller_manager update */ 80 | virtual void update(const ros::Time& now, const ros::Duration& dt); 81 | 82 | /** \brief Get a list of joints this controls. */ 83 | virtual std::vector getCommandedNames(); 84 | virtual std::vector getClaimedNames(); 85 | 86 | private: 87 | /** \brief Callback for goal */ 88 | void executeCb(const control_msgs::GripperCommandGoalConstPtr& goal); 89 | 90 | bool initialized_; 91 | std::vector joint_names_; 92 | robot_controllers::ControllerManager* manager_; 93 | 94 | ubr1_gazebo::GazeboJointHandlePtr left_; 95 | ubr1_gazebo::GazeboJointHandlePtr right_; 96 | 97 | // The goal pose for the gripper 98 | double goal_; 99 | double fudge_scale_; 100 | 101 | boost::shared_ptr server_; 102 | }; 103 | 104 | } // namespace ubr1_gazebo_controllers 105 | 106 | #endif // UBR1_GAZEBO_CONTROLLERS_SIMULATED_GRIPPER_CONTROLLER_H_ 107 | -------------------------------------------------------------------------------- /ubr1_gazebo/include/ubr1_gazebo/ubr1_gazebo_plugin.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Michael Ferguson 5 | * Copyright (c) 2013-2014, Unbounded Robotics Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Unbounded Robotics nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | /* Authors: John Hsu, Michael Ferguson */ 37 | 38 | #ifndef UBR1_GAZEBO_UBR1_GAZEBO_PLUGIN_H 39 | #define UBR1_GAZEBO_UBR1_GAZEBO_PLUGIN_H 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | #include 48 | 49 | #include 50 | 51 | namespace gazebo 52 | { 53 | 54 | class UBR1GazeboPlugin : public ModelPlugin 55 | { 56 | public: 57 | UBR1GazeboPlugin(); 58 | ~UBR1GazeboPlugin(); 59 | virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); 60 | virtual void Init(); 61 | 62 | private: 63 | 64 | /** \brief Callback to handle periodic updating for Gazebo */ 65 | void OnUpdate(); 66 | 67 | event::ConnectionPtr updateConnection; 68 | 69 | physics::ModelPtr model; 70 | std::vector jointNames; 71 | 72 | common::Time prevUpdateTime; 73 | 74 | robot_controllers::ControllerManager* manager_; 75 | std::vector joints_; 76 | 77 | ros::Publisher joint_state_pub_; 78 | ros::NodeHandle nh_; 79 | 80 | ros::Time last_publish_; 81 | }; 82 | 83 | } // namespace gazebo 84 | 85 | #endif // UBR1_GAZEBO_UBR1_GAZEBO_PLUGIN_H 86 | -------------------------------------------------------------------------------- /ubr1_gazebo/launch/include/head_camera.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | filter_field_name: z 35 | filter_limit_min: 1.0 36 | filter_limit_max: 3.0 37 | filter_limit_negative: False 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /ubr1_gazebo/launch/include/simulation.ubr1.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /ubr1_gazebo/launch/simulation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /ubr1_gazebo/launch/simulation_grasping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /ubr1_gazebo/launch/simulation_home.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /ubr1_gazebo/models/table_30/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | table_30 4 | 0.1.0 5 | 6 | Michael Ferguson 7 | fergs@unboundedrobotics.com 8 | 9 | table.sdf 10 | 11 | Standard 30" tall table 12 | 13 | 14 | -------------------------------------------------------------------------------- /ubr1_gazebo/models/table_30/table.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 6 | 7 | 0.000000 0.000000 0.76200 0.000000 0.000000 0.000000 8 | 9 | 10 | 1.000000 0.800000 0.030000 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 0.000000 0.000000 0.762000 0.000000 0.000000 0.000000 25 | 26 | 27 | 1.000000 0.800000 0.030000 28 | 29 | 30 | 31 | 35 | 36 | 37 | 38 | 0.480000 0.380000 0.381000 0.000000 0.000000 0.000000 39 | 40 | 41 | 0.020000 42 | 0.762000 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 0.480000 0.380000 0.381000 0.000000 0.000000 0.000000 57 | 58 | 59 | 0.020000 60 | 0.762000 61 | 62 | 63 | 64 | 68 | 69 | 70 | 71 | 0.480000 -0.380000 0.381000 0.000000 0.000000 0.000000 72 | 73 | 74 | 0.020000 75 | 0.762000 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 0.480000 -0.380000 0.381000 0.000000 0.000000 0.000000 90 | 91 | 92 | 0.020000 93 | 0.762000 94 | 95 | 96 | 97 | 101 | 102 | 103 | 104 | -0.480000 -0.380000 0.381000 0.000000 0.000000 0.000000 105 | 106 | 107 | 0.020000 108 | 0.762000 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | -0.480000 -0.380000 0.381000 0.000000 0.000000 0.000000 123 | 124 | 125 | 0.020000 126 | 0.762000 127 | 128 | 129 | 130 | 134 | 135 | 136 | 137 | -0.480000 0.380000 0.381000 0.000000 0.000000 0.000000 138 | 139 | 140 | 0.020000 141 | 0.762000 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | -0.480000 0.380000 0.381000 0.000000 0.000000 0.000000 156 | 157 | 158 | 0.020000 159 | 0.762000 160 | 161 | 162 | 163 | 167 | 168 | 169 | 170 | 0.000000 171 | 0.000000 172 | 173 | 0 174 | 0 175 | 1 176 | 177 | 178 | 179 | -------------------------------------------------------------------------------- /ubr1_gazebo/models/ubr_cube/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | ubr_cube 4 | 0.1.0 5 | 6 | Michael Ferguson 7 | fergs@unboundedrobotics.com 8 | 9 | ubr_cube.sdf 10 | 11 | The ubiquitous Unbounded Robotics Standard Orange Grasping Cube. 12 | 13 | 14 | -------------------------------------------------------------------------------- /ubr1_gazebo/models/ubr_cube/ubr_cube.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 5 | 6 | 7 | 0.25 8 | 9 | 0.001 10 | 0.000000 11 | 0.000000 12 | 0.001 13 | 0.000000 14 | 0.001 15 | 16 | 17 | 18 | 19 | 20 | 0.060 0.060 0.060 21 | 22 | 23 | 24 | 25 | 26 | 30.0 27 | 30.0 28 | 29 | 30 | 31 | 32 | 1000000.0 33 | 100.0 34 | 1.0 35 | 0.002 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 0.060 0.060 0.060 44 | 45 | 46 | 47 | 51 | 52 | 53 | 54 | 0.000000 55 | 0.000000 56 | 57 | 0 58 | 0 59 | 1 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /ubr1_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ubr1_gazebo 5 | 0.3.0 6 | 7 | Gazebo package for the UBR-1. 8 | 9 | Michael Ferguson 10 | Michael Ferguson 11 | BSD 12 | http://ros.org/wiki/ubr1_gazebo 13 | 14 | catkin 15 | 16 | control_toolbox 17 | gazebo_ros 18 | robot_controllers 19 | robot_controllers_interface 20 | 21 | depth_image_proc 22 | image_proc 23 | nodelet 24 | xacro 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /ubr1_gazebo/scripts/stop_controllers.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from __future__ import print_function 4 | 5 | import rospy 6 | import actionlib 7 | 8 | from ubr_msgs.srv import * 9 | 10 | if __name__ == '__main__': 11 | rospy.init_node('stop_controllers') 12 | rospy.wait_for_service('gazebo/update_controllers') 13 | service = rospy.ServiceProxy('gazebo/update_controllers', UpdateControllers) 14 | try: 15 | start = list() 16 | start.append('arm_controller/gravity_compensation') 17 | stop = list() 18 | stop.append('arm_controller/follow_joint_trajectory') 19 | stop.append('arm_with_torso_controller/follow_joint_trajectory') 20 | stop.append('torso_controller/follow_joint_trajectory') 21 | resp = service(start, stop) 22 | except rospy.ServiceException as e: 23 | print('Failed to stop controllers') 24 | -------------------------------------------------------------------------------- /ubr1_gazebo/src/simulated_bellows_controller.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Michael Ferguson 5 | * Copyright (c) 2014, Unbounded Robotics Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Unbounded Robotics nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | // Author: Michael Ferguson 37 | 38 | #include 39 | #include 40 | 41 | PLUGINLIB_EXPORT_CLASS(ubr1_gazebo_controllers::SimulatedBellowsController, robot_controllers::Controller) 42 | 43 | namespace ubr1_gazebo_controllers 44 | { 45 | 46 | int SimulatedBellowsController::init(ros::NodeHandle& nh, robot_controllers::ControllerManager* manager) 47 | { 48 | // We absolutely need access to the controller manager 49 | if (!manager) 50 | { 51 | initialized_ = false; 52 | return -1; 53 | } 54 | 55 | robot_controllers::Controller::init(nh, manager); 56 | 57 | // Get Joint Handles 58 | bellows_ = manager->getJointHandle("bellows_joint"); 59 | torso_lift_ = manager->getJointHandle("torso_lift_joint"); 60 | 61 | initialized_ = true; 62 | 63 | // Start this controller 64 | manager->requestStart(getName()); 65 | 66 | return 0; 67 | } 68 | 69 | bool SimulatedBellowsController::start() 70 | { 71 | if (!initialized_) 72 | { 73 | ROS_ERROR_NAMED("SimulatedBellowsController", 74 | "Unable to start, not initialized."); 75 | return false; 76 | } 77 | 78 | return true; 79 | } 80 | 81 | bool SimulatedBellowsController::stop(bool force) 82 | { 83 | if (!initialized_) 84 | return true; 85 | 86 | if (force) 87 | return true; 88 | 89 | // If we preempt -- the bellows will fall -- and the world will end! 90 | return false; 91 | } 92 | 93 | bool SimulatedBellowsController::reset() 94 | { 95 | return true; 96 | } 97 | 98 | void SimulatedBellowsController::update(const ros::Time& now, const ros::Duration& dt) 99 | { 100 | // I warned you this controller was stupid 101 | bellows_->setPosition(torso_lift_->getPosition() / -2.0, 0.0, 0.0); 102 | } 103 | 104 | std::vector SimulatedBellowsController::getCommandedNames() 105 | { 106 | std::vector names; 107 | names.push_back("bellows_joint"); 108 | return names; 109 | } 110 | 111 | std::vector SimulatedBellowsController::getClaimedNames() 112 | { 113 | std::vector names; 114 | // No claimed names 115 | return names; 116 | } 117 | 118 | } // namespace ubr1_gazebo_controllers 119 | -------------------------------------------------------------------------------- /ubr1_gazebo/src/ubr1_gazebo_plugin.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Michael Ferguson 5 | * Copyright (c) 2013-2014, Unbounded Robotics Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Unbounded Robotics nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | // Authors: John Hsu, Michael Ferguson 37 | 38 | #include 39 | #include 40 | 41 | using namespace gazebo; 42 | GZ_REGISTER_MODEL_PLUGIN(UBR1GazeboPlugin) 43 | 44 | UBR1GazeboPlugin::UBR1GazeboPlugin() : nh_() 45 | { 46 | } 47 | 48 | UBR1GazeboPlugin::~UBR1GazeboPlugin() 49 | { 50 | delete this->manager_; 51 | } 52 | 53 | void UBR1GazeboPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr /*_sdf*/) 54 | { 55 | this->model = _model; 56 | last_publish_ = ros::Time(this->model->GetWorld()->SimTime().Double()); 57 | 58 | this->updateConnection = event::Events::ConnectWorldUpdateBegin( 59 | boost::bind(&UBR1GazeboPlugin::OnUpdate, this)); 60 | } 61 | 62 | void UBR1GazeboPlugin::Init() 63 | { 64 | // Create ControllerManager instance 65 | this->manager_ = new robot_controllers::ControllerManager(); 66 | 67 | // Setup joints 68 | gazebo::physics::Joint_V joints = this->model->GetJoints(); 69 | for (auto it = joints.begin(); it != joints.end(); ++it) 70 | { 71 | joints_.emplace_back(new ubr1_gazebo::GazeboJointHandle(*it)); 72 | robot_controllers::JointHandlePtr jh = joints_.back(); 73 | this->manager_->addJointHandle(jh); 74 | } 75 | 76 | // Setup controllers 77 | ros::NodeHandle nh("~"); 78 | this->manager_->init(nh); 79 | 80 | // Publish joint states only after controllers are fully ready 81 | this->joint_state_pub_ = nh_.advertise("joint_states", 10); 82 | 83 | ROS_INFO("Finished initializing UBR1GazeboPlugin"); 84 | } 85 | 86 | void UBR1GazeboPlugin::OnUpdate() 87 | { 88 | // Don't try to update/publish if we are shutting down 89 | if (!ros::ok()) return; 90 | 91 | // Get time and timestep for controllers 92 | common::Time currTime = this->model->GetWorld()->SimTime(); 93 | common::Time stepTime = currTime - this->prevUpdateTime; 94 | this->prevUpdateTime = currTime; 95 | double dt = stepTime.Double(); 96 | ros::Time now = ros::Time(currTime.Double()); 97 | 98 | // Update controllers 99 | this->manager_->update(now, ros::Duration(dt)); 100 | 101 | // Copy commands to Gazebo 102 | for (auto joint : joints_) 103 | { 104 | joint->update(now, ros::Duration(dt)); 105 | } 106 | 107 | // Limit publish rate 108 | if (now - last_publish_ < ros::Duration(0.01)) 109 | return; 110 | 111 | // Publish joint_state message 112 | sensor_msgs::JointState js; 113 | js.header.stamp = ros::Time(currTime.Double()); 114 | gazebo::physics::Joint_V joints = this->model->GetJoints(); 115 | for (gazebo::physics::Joint_V::iterator it = joints.begin(); it != joints.end(); ++it) 116 | { 117 | robot_controllers::JointHandlePtr j = this->manager_->getJointHandle((*it)->GetName()); 118 | js.name.push_back((*it)->GetName()); 119 | js.position.push_back(j->getPosition()); 120 | js.velocity.push_back(j->getVelocity()); 121 | js.effort.push_back(j->getEffort()); 122 | } 123 | joint_state_pub_.publish(js); 124 | 125 | last_publish_ = now; 126 | } 127 | 128 | -------------------------------------------------------------------------------- /ubr1_gazebo/test/pick_up_cube_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /ubr1_gazebo/test/pick_up_cube_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import rosbag 5 | import actionlib 6 | from control_msgs.msg import * 7 | from trajectory_msgs.msg import * 8 | 9 | class PickUpCubeTest: 10 | 11 | def __init__(self): 12 | rospy.loginfo("Waiting for arm_controller...") 13 | self.arm_client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction) 14 | self.arm_client.wait_for_server() 15 | rospy.loginfo("...connected.") 16 | 17 | rospy.loginfo("Waiting for gripper_controller...") 18 | self.gripper_client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction) 19 | self.gripper_client.wait_for_server() 20 | rospy.loginfo("...connected") 21 | 22 | def run(self): 23 | bag = rosbag.Bag("pick_up_cube_trajectories.bag") 24 | msg_count = 0 25 | for topic, msg, t in bag.read_messages(): 26 | # send each trajectory 27 | self.arm_client.send_goal(msg.goal) 28 | self.arm_client.wait_for_result(rospy.Duration(15.0)) 29 | msg_count += 1 30 | 31 | # maybe close the gripper? 32 | if msg_count == 2: 33 | goal = GripperCommandGoal() 34 | goal.command.max_effort = 28.0 35 | goal.command.position = 0.0 36 | self.gripper_client.send_goal(goal) 37 | self.gripper_client.wait_for_result(rospy.Duration(15.0)) 38 | 39 | if __name__ == "__main__": 40 | rospy.init_node("pick_up_cube_test") 41 | test = PickUpCubeTest() 42 | test.run() 43 | 44 | -------------------------------------------------------------------------------- /ubr1_gazebo/test/pick_up_cube_trajectories.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_gazebo/test/pick_up_cube_trajectories.bag -------------------------------------------------------------------------------- /ubr1_gazebo/test/scripts/generate_grasp_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import copy 4 | import math 5 | import sys 6 | 7 | import rospy 8 | import actionlib 9 | from moveit_python import * 10 | from moveit_python.geometry import * 11 | 12 | from geometry_msgs.msg import * 13 | from grasping_msgs.msg import * 14 | from moveit_msgs.msg import * 15 | from trajectory_msgs.msg import * 16 | 17 | def create_cube(interface): 18 | interface.addCube("cube", 0.06, 0.25, -0.25, 0.03) 19 | return interface._objects["cube"] 20 | 21 | def make_gripper_posture(pose): 22 | t = JointTrajectory() 23 | t.joint_names = ["left_gripper_joint", "left_gripper_joint"] 24 | tp = JointTrajectoryPoint() 25 | tp.positions = [pose/2.0 for j in t.joint_names] 26 | tp.effort = [28.0 for j in t.joint_names] 27 | t.points.append(tp) 28 | return t 29 | 30 | def make_gripper_translation(min_dist, desired, axis=1.0): 31 | g = GripperTranslation() 32 | g.direction.vector.x = axis 33 | g.direction.header.frame_id = "wrist_roll_link" 34 | g.min_distance = min_dist 35 | g.desired_distance = desired 36 | return g 37 | 38 | def make_grasps(pose_stamped): 39 | grasps = list() 40 | g = Grasp() 41 | g.pre_grasp_posture = make_gripper_posture(0.09) 42 | g.grasp_posture = make_gripper_posture(0.0) 43 | g.pre_grasp_approach = make_gripper_translation(0.1, 0.15) 44 | g.post_grasp_retreat = make_gripper_translation(0.1, 0.15, -1.0) 45 | g.grasp_pose = pose_stamped 46 | g.grasp_pose.pose.position.z += .110 + .01 47 | 48 | q = quaternion_from_euler(0, 1.57, 0.0) 49 | g.grasp_pose.pose.orientation.x = q[0] 50 | g.grasp_pose.pose.orientation.y = q[1] 51 | g.grasp_pose.pose.orientation.z = q[2] 52 | g.grasp_pose.pose.orientation.w = q[3] 53 | g.id = "overhead" 54 | g.grasp_quality = 1.0 55 | grasps.append(g) 56 | return grasps 57 | 58 | if __name__=="__main__": 59 | rospy.init_node("generate_grasp_test") 60 | 61 | scene = PlanningSceneInterface("base_link") 62 | pickplace = PickPlaceInterface("arm", "gripper", verbose = True) 63 | 64 | cube = create_cube(scene) 65 | pose_stamped = PoseStamped() 66 | pose_stamped.pose = cube.primitive_poses[0] 67 | pose_stamped.header.frame_id = "base_link" 68 | grasps = make_grasps(pose_stamped) 69 | 70 | rospy.loginfo("Beginning to pick.") 71 | success, pick_result = pickplace.pick_with_retry("cube", grasps, scene = scene) 72 | 73 | -------------------------------------------------------------------------------- /ubr1_gazebo/ubr1_gazebo_controllers.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | Simulated Gripper Controller 7 | 8 | 9 | 12 | Simulated Bellows Controller 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /ubr1_gazebo/worlds/cube_on_ground.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0.000000 0.000000 -9.810000 6 | 7 | 8 | 1.000000 9 | 10 | 11 | 12 | 13 | 1 14 | 0 0 10 0 -0 0 15 | 0.8 0.8 0.8 1 16 | 0.2 0.2 0.2 1 17 | 18 | 1000 19 | 0.9 20 | 0.01 21 | 0.001 22 | 23 | -0.5 0.1 -0.9 24 | 25 | 26 | 27 | model://ground_plane 28 | 29 | 30 | 31 | model://ubr_cube 32 | 0.25 -0.25 0.03 0 0 0 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /ubr1_gazebo/worlds/cube_on_table.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 1 6 | 0 0 10 0 -0 0 7 | 0.8 0.8 0.8 1 8 | 0.2 0.2 0.2 1 9 | 10 | 1000 11 | 0.9 12 | 0.01 13 | 0.001 14 | 15 | -0.5 0.1 -0.9 16 | 17 | 18 | 19 | model://ground_plane 20 | 21 | 22 | 23 | model://table_30 24 | table1 25 | 0.8 0 0 0 0 1.57 26 | 27 | 28 | 29 | model://ubr_cube 30 | 0.50 0.15 0.83 0 0 0 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /ubr1_moveit/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | urdf: 3 | package: ubr1_description 4 | relative_path: robots/ubr1_robot.urdf 5 | srdf: 6 | relative_path: config/ubr1.srdf 7 | package_settings: 8 | author_name: Michael Ferguson 9 | author_email: mfergs7@gmail.com 10 | generated_timestamp: 1727277957 11 | control_xacro: 12 | command: 13 | - position 14 | state: 15 | - position 16 | - velocity 17 | modified_urdf: 18 | xacros: 19 | - control_xacro 20 | control_xacro: 21 | command: 22 | - position 23 | state: 24 | - position 25 | - velocity -------------------------------------------------------------------------------- /ubr1_moveit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(ubr1_moveit) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | ament_package() 7 | 8 | install( 9 | DIRECTORY launch 10 | DESTINATION share/${PROJECT_NAME} 11 | PATTERN "setup_assistant.launch" EXCLUDE) 12 | install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) 13 | install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) 14 | -------------------------------------------------------------------------------- /ubr1_moveit/config/initial_positions.yaml: -------------------------------------------------------------------------------- 1 | # Default initial positions for ubr1's ros2_control fake system 2 | 3 | initial_positions: 4 | elbow_flex_joint: 0 5 | forearm_roll_joint: 0 6 | left_gripper_joint: 0 7 | right_gripper_joint: 0 8 | shoulder_lift_joint: 0 9 | shoulder_pan_joint: 0 10 | torso_lift_joint: 0 11 | upperarm_roll_joint: 0 12 | wrist_flex_joint: 0 13 | wrist_roll_joint: 0 -------------------------------------------------------------------------------- /ubr1_moveit/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | 3 | # For beginners, we downscale velocity and acceleration limits. 4 | # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. 5 | default_velocity_scaling_factor: 0.1 6 | default_acceleration_scaling_factor: 0.1 7 | 8 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 9 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 10 | joint_limits: 11 | elbow_flex_joint: 12 | has_velocity_limits: true 13 | max_velocity: 1.3089969400000001 14 | has_acceleration_limits: true 15 | max_acceleration: 1.5 16 | forearm_roll_joint: 17 | has_velocity_limits: true 18 | max_velocity: 2.0943950999999998 19 | has_acceleration_limits: true 20 | max_acceleration: 1.5 21 | left_gripper_joint: 22 | has_velocity_limits: true 23 | max_velocity: 0.10000000000000001 24 | has_acceleration_limits: true 25 | max_acceleration: 1.0 26 | right_gripper_joint: 27 | has_velocity_limits: true 28 | max_velocity: 0.10000000000000001 29 | has_acceleration_limits: true 30 | max_acceleration: 1.0 31 | shoulder_lift_joint: 32 | has_velocity_limits: true 33 | max_velocity: 1.3089969400000001 34 | has_acceleration_limits: true 35 | max_acceleration: 1.5 36 | shoulder_pan_joint: 37 | has_velocity_limits: true 38 | max_velocity: 1.3089969400000001 39 | has_acceleration_limits: true 40 | max_acceleration: 1.5 41 | torso_lift_joint: 42 | has_velocity_limits: true 43 | max_velocity: 0.10000000000000001 44 | has_acceleration_limits: true 45 | max_acceleration: 1.0 46 | upperarm_roll_joint: 47 | has_velocity_limits: true 48 | max_velocity: 2.0943950999999998 49 | has_acceleration_limits: true 50 | max_acceleration: 1.5 51 | wrist_flex_joint: 52 | has_velocity_limits: true 53 | max_velocity: 2.0943950999999998 54 | has_acceleration_limits: true 55 | max_acceleration: 1.5 56 | wrist_roll_joint: 57 | has_velocity_limits: true 58 | max_velocity: 2.0943950999999998 59 | has_acceleration_limits: true 60 | max_acceleration: 1.5 -------------------------------------------------------------------------------- /ubr1_moveit/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | arm: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.0050000000000000001 4 | kinematics_solver_timeout: 0.0050000000000000001 5 | arm_with_torso: 6 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 7 | kinematics_solver_search_resolution: 0.0050000000000000001 8 | kinematics_solver_timeout: 0.0050000000000000001 -------------------------------------------------------------------------------- /ubr1_moveit/config/moveit.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Name: Displays 4 | Property Tree Widget: 5 | Expanded: 6 | - /MotionPlanning1 7 | - Class: rviz_common/Help 8 | Name: Help 9 | - Class: rviz_common/Views 10 | Name: Views 11 | Visualization Manager: 12 | Displays: 13 | - Class: rviz_default_plugins/Grid 14 | Name: Grid 15 | Value: true 16 | - Class: moveit_rviz_plugin/MotionPlanning 17 | Name: MotionPlanning 18 | Planned Path: 19 | Loop Animation: true 20 | State Display Time: 0.05 s 21 | Trajectory Topic: display_planned_path 22 | Planning Scene Topic: monitored_planning_scene 23 | Robot Description: robot_description 24 | Scene Geometry: 25 | Scene Alpha: 1 26 | Scene Robot: 27 | Robot Alpha: 0.5 28 | Value: true 29 | Global Options: 30 | Fixed Frame: base_link 31 | Tools: 32 | - Class: rviz_default_plugins/Interact 33 | - Class: rviz_default_plugins/MoveCamera 34 | - Class: rviz_default_plugins/Select 35 | Value: true 36 | Views: 37 | Current: 38 | Class: rviz_default_plugins/Orbit 39 | Distance: 2.0 40 | Focal Point: 41 | X: -0.1 42 | Y: 0.25 43 | Z: 0.30 44 | Name: Current View 45 | Pitch: 0.5 46 | Target Frame: base_link 47 | Yaw: -0.623 48 | Window Geometry: 49 | Height: 975 50 | QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 51 | Width: 1200 52 | -------------------------------------------------------------------------------- /ubr1_moveit/config/moveit_controllers.yaml: -------------------------------------------------------------------------------- 1 | # MoveIt uses this configuration for controller management 2 | 3 | moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager 4 | 5 | moveit_simple_controller_manager: 6 | controller_names: 7 | - arm_controller 8 | - arm_with_torso_controller 9 | - gripper_controller 10 | 11 | arm_controller: 12 | type: FollowJointTrajectory 13 | joints: 14 | - shoulder_pan_joint 15 | - shoulder_lift_joint 16 | - upperarm_roll_joint 17 | - elbow_flex_joint 18 | - forearm_roll_joint 19 | - wrist_flex_joint 20 | - wrist_roll_joint 21 | action_ns: follow_joint_trajectory 22 | default: true 23 | arm_with_torso_controller: 24 | type: FollowJointTrajectory 25 | joints: 26 | - torso_lift_joint 27 | - shoulder_pan_joint 28 | - shoulder_lift_joint 29 | - upperarm_roll_joint 30 | - elbow_flex_joint 31 | - forearm_roll_joint 32 | - wrist_flex_joint 33 | - wrist_roll_joint 34 | action_ns: follow_joint_trajectory 35 | default: true 36 | gripper_controller: 37 | type: GripperCommand 38 | parallel: true 39 | joints: 40 | - left_gripper_joint 41 | - right_gripper_joint 42 | action_ns: command 43 | default: true -------------------------------------------------------------------------------- /ubr1_moveit/config/pilz_cartesian_limits.yaml: -------------------------------------------------------------------------------- 1 | # Limits for the Pilz planner 2 | cartesian_limits: 3 | max_trans_vel: 1.0 4 | max_trans_acc: 2.25 5 | max_trans_dec: -5.0 6 | max_rot_vel: 1.57 7 | -------------------------------------------------------------------------------- /ubr1_moveit/config/ros2_controllers.yaml: -------------------------------------------------------------------------------- 1 | # This config file is used by ros2_control 2 | controller_manager: 3 | ros__parameters: 4 | update_rate: 100 # Hz 5 | 6 | arm_controller: 7 | type: joint_trajectory_controller/JointTrajectoryController 8 | 9 | 10 | arm_with_torso_controller: 11 | type: joint_trajectory_controller/JointTrajectoryController 12 | 13 | 14 | gripper_controller: 15 | type: position_controllers/GripperActionController 16 | 17 | 18 | joint_state_broadcaster: 19 | type: joint_state_broadcaster/JointStateBroadcaster 20 | 21 | arm_controller: 22 | ros__parameters: 23 | joints: 24 | - shoulder_pan_joint 25 | - shoulder_lift_joint 26 | - upperarm_roll_joint 27 | - elbow_flex_joint 28 | - forearm_roll_joint 29 | - wrist_flex_joint 30 | - wrist_roll_joint 31 | command_interfaces: 32 | - position 33 | state_interfaces: 34 | - position 35 | - velocity 36 | allow_nonzero_velocity_at_trajectory_end: true 37 | arm_with_torso_controller: 38 | ros__parameters: 39 | joints: 40 | - torso_lift_joint 41 | - shoulder_pan_joint 42 | - shoulder_lift_joint 43 | - upperarm_roll_joint 44 | - elbow_flex_joint 45 | - forearm_roll_joint 46 | - wrist_flex_joint 47 | - wrist_roll_joint 48 | command_interfaces: 49 | - position 50 | state_interfaces: 51 | - position 52 | - velocity 53 | allow_nonzero_velocity_at_trajectory_end: true 54 | gripper_controller: 55 | ros__parameters: 56 | joint: left_gripper_joint -------------------------------------------------------------------------------- /ubr1_moveit/config/sensors_3d.yaml: -------------------------------------------------------------------------------- 1 | sensors: [] 2 | # - default_sensor 3 | default_sensor: 4 | far_clipping_plane_distance: 5.0 5 | filtered_cloud_topic: "" 6 | image_topic: /head_camera/depth_downsample/image_raw 7 | max_update_rate: 5.0 8 | near_clipping_plane_distance: 0.0 9 | padding_offset: 0.0 10 | padding_scale: 0.0 11 | queue_size: 1 12 | sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater 13 | shadow_threshold: 0.0 -------------------------------------------------------------------------------- /ubr1_moveit/config/ubr1.ros2_control.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | mock_components/GenericSystem 10 | 11 | 12 | 13 | 14 | ${initial_positions['shoulder_pan_joint']} 15 | 16 | 17 | 18 | 19 | 20 | 21 | ${initial_positions['shoulder_lift_joint']} 22 | 23 | 24 | 25 | 26 | 27 | 28 | ${initial_positions['upperarm_roll_joint']} 29 | 30 | 31 | 32 | 33 | 34 | 35 | ${initial_positions['elbow_flex_joint']} 36 | 37 | 38 | 39 | 40 | 41 | 42 | ${initial_positions['forearm_roll_joint']} 43 | 44 | 45 | 46 | 47 | 48 | 49 | ${initial_positions['wrist_flex_joint']} 50 | 51 | 52 | 53 | 54 | 55 | 56 | ${initial_positions['wrist_roll_joint']} 57 | 58 | 59 | 60 | 61 | 62 | 63 | ${initial_positions['torso_lift_joint']} 64 | 65 | 66 | 67 | 68 | 69 | 70 | ${initial_positions['left_gripper_joint']} 71 | 72 | 73 | 74 | 75 | 76 | 77 | ${initial_positions['right_gripper_joint']} 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /ubr1_moveit/config/ubr1.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /ubr1_moveit/launch/demo.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_demo_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder("ubr1", package_name="ubr1_moveit").to_moveit_configs() 7 | return generate_demo_launch(moveit_config) 8 | -------------------------------------------------------------------------------- /ubr1_moveit/launch/move_group.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_move_group_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder("ubr1", package_name="ubr1_moveit").to_moveit_configs() 7 | moveit_config.move_group_capabilities["capabilities"] = ["move_group/ExecuteTaskSolutionCapability"] 8 | return generate_move_group_launch(moveit_config) 9 | -------------------------------------------------------------------------------- /ubr1_moveit/launch/moveit_rviz.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_moveit_rviz_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder("ubr1", package_name="ubr1_moveit").to_moveit_configs() 7 | return generate_moveit_rviz_launch(moveit_config) 8 | -------------------------------------------------------------------------------- /ubr1_moveit/launch/rsp.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_rsp_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder("ubr1", package_name="ubr1_moveit").to_moveit_configs() 7 | return generate_rsp_launch(moveit_config) 8 | -------------------------------------------------------------------------------- /ubr1_moveit/launch/setup_assistant.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_setup_assistant_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder("ubr1", package_name="ubr1_moveit").to_moveit_configs() 7 | return generate_setup_assistant_launch(moveit_config) 8 | -------------------------------------------------------------------------------- /ubr1_moveit/launch/spawn_controllers.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_spawn_controllers_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder("ubr1", package_name="ubr1_moveit").to_moveit_configs() 7 | return generate_spawn_controllers_launch(moveit_config) 8 | -------------------------------------------------------------------------------- /ubr1_moveit/launch/static_virtual_joint_tfs.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder("ubr1", package_name="ubr1_moveit").to_moveit_configs() 7 | return generate_static_virtual_joint_tfs_launch(moveit_config) 8 | -------------------------------------------------------------------------------- /ubr1_moveit/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ubr1_moveit 5 | 0.3.0 6 | 7 | An automatically generated package with all the configuration and launch files for using the ubr1 with the MoveIt Motion Planning Framework 8 | 9 | Michael Ferguson 10 | 11 | BSD-3-Clause 12 | 13 | http://moveit.ros.org/ 14 | https://github.com/moveit/moveit2/issues 15 | https://github.com/moveit/moveit2 16 | 17 | Michael Ferguson 18 | 19 | ament_cmake 20 | 21 | moveit_ros_move_group 22 | moveit_kinematics 23 | moveit_planners 24 | moveit_simple_controller_manager 25 | joint_state_publisher 26 | joint_state_publisher_gui 27 | tf2_ros 28 | xacro 29 | 31 | 32 | 33 | controller_manager 34 | moveit_configs_utils 35 | moveit_ros_move_group 36 | moveit_ros_visualization 37 | moveit_setup_assistant 38 | robot_state_publisher 39 | rviz2 40 | rviz_common 41 | rviz_default_plugins 42 | tf2_ros 43 | ubr1_description 44 | xacro 45 | 46 | 47 | 48 | ament_cmake 49 | 50 | 51 | -------------------------------------------------------------------------------- /ubr1_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ubr1_navigation) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(cv_bridge REQUIRED) 6 | find_package(image_transport REQUIRED) 7 | find_package(nav2_costmap_2d REQUIRED) 8 | find_package(pluginlib REQUIRED) 9 | find_package(sensor_msgs REQUIRED) 10 | find_package(tf2_ros REQUIRED) 11 | find_package(OpenCV REQUIRED) 12 | 13 | include_directories( 14 | include 15 | ${OpenCV_INCLUDE_DIRS} 16 | ) 17 | 18 | add_library(depth_layer SHARED 19 | src/depth_layer.cpp 20 | ) 21 | ament_target_dependencies(depth_layer 22 | cv_bridge 23 | nav2_costmap_2d 24 | pluginlib 25 | sensor_msgs 26 | tf2_ros 27 | image_transport 28 | ) 29 | target_link_libraries(depth_layer 30 | ${OpenCV_LIBRARIES} 31 | ) 32 | 33 | install( 34 | DIRECTORY behavior_trees config launch maps 35 | DESTINATION share/${PROJECT_NAME}/ 36 | ) 37 | 38 | install( 39 | FILES costmap_plugins.xml 40 | DESTINATION share/${PROJECT_NAME} 41 | ) 42 | 43 | install( 44 | PROGRAMS scripts/tilt_head.py 45 | DESTINATION lib/${PROJECT_NAME} 46 | ) 47 | 48 | install( 49 | TARGETS depth_layer 50 | ARCHIVE DESTINATION lib 51 | LIBRARY DESTINATION lib 52 | RUNTIME DESTINATION bin 53 | ) 54 | 55 | pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml) 56 | ament_package() 57 | -------------------------------------------------------------------------------- /ubr1_navigation/behavior_trees/default.xml: -------------------------------------------------------------------------------- 1 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /ubr1_navigation/config/mapper_params_online_sync.yaml: -------------------------------------------------------------------------------- 1 | slam_toolbox: 2 | ros__parameters: 3 | 4 | # Plugin params 5 | solver_plugin: solver_plugins::CeresSolver 6 | ceres_linear_solver: SPARSE_NORMAL_CHOLESKY 7 | ceres_preconditioner: SCHUR_JACOBI 8 | ceres_trust_strategy: LEVENBERG_MARQUARDT 9 | ceres_dogleg_type: TRADITIONAL_DOGLEG 10 | ceres_loss_function: None 11 | 12 | # ROS Parameters 13 | odom_frame: odom 14 | map_frame: map 15 | base_frame: base_link 16 | scan_topic: /base_scan 17 | mode: mapping 18 | 19 | debug_logging: false 20 | throttle_scans: 1 21 | transform_publish_period: 0.02 #if 0 never publishes odometry 22 | map_update_interval: 5.0 23 | resolution: 0.05 24 | max_laser_range: 20.0 #for rastering images 25 | minimum_time_interval: 0.5 26 | transform_timeout: 0.2 27 | tf_buffer_duration: 30. 28 | stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps 29 | enable_interactive_mode: true 30 | 31 | # General Parameters 32 | use_scan_matching: true 33 | use_scan_barycenter: true 34 | minimum_travel_distance: 0.5 35 | minimum_travel_heading: 0.5 36 | scan_buffer_size: 10 37 | scan_buffer_maximum_scan_distance: 10.0 38 | link_match_minimum_response_fine: 0.1 39 | link_scan_maximum_distance: 1.5 40 | loop_search_maximum_distance: 3.0 41 | do_loop_closing: true 42 | loop_match_minimum_chain_size: 10 43 | loop_match_maximum_variance_coarse: 3.0 44 | loop_match_minimum_response_coarse: 0.35 45 | loop_match_minimum_response_fine: 0.45 46 | 47 | # Correlation Parameters - Correlation Parameters 48 | correlation_search_space_dimension: 0.5 49 | correlation_search_space_resolution: 0.01 50 | correlation_search_space_smear_deviation: 0.1 51 | 52 | # Correlation Parameters - Loop Closure Parameters 53 | loop_search_space_dimension: 8.0 54 | loop_search_space_resolution: 0.05 55 | loop_search_space_smear_deviation: 0.03 56 | 57 | # Scan Matcher Parameters 58 | distance_variance_penalty: 0.5 59 | angle_variance_penalty: 1.0 60 | 61 | fine_search_angle_offset: 0.00349 62 | coarse_search_angle_offset: 0.349 63 | coarse_angle_resolution: 0.0349 64 | minimum_angle_penalty: 0.9 65 | minimum_distance_penalty: 0.5 66 | use_response_expansion: true 67 | -------------------------------------------------------------------------------- /ubr1_navigation/config/nav2_params.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | alpha1: 0.3 4 | alpha2: 0.2 5 | alpha3: 0.1 6 | alpha4: 0.2 7 | base_frame_id: "base_link" 8 | global_frame_id: "map" 9 | laser_model_type: "likelihood_field" 10 | max_beams: 100 11 | max_particles: 2000 12 | min_particles: 500 13 | odom_frame_id: "odom" 14 | robot_model_type: "nav2_amcl::DifferentialMotionModel" 15 | tf_broadcast: true 16 | transform_tolerance: 0.5 17 | z_hit: 0.9 18 | z_max: 0.05 19 | z_rand: 0.1 20 | z_short: 0.05 21 | 22 | bt_navigator: 23 | ros__parameters: 24 | global_frame: map 25 | robot_base_frame: base_link 26 | odom_topic: /odom 27 | navigators: ["navigate_to_pose", "navigate_through_poses"] 28 | navigate_to_pose: 29 | plugin: "nav2_bt_navigator::NavigateToPoseNavigator" 30 | navigate_through_poses: 31 | plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" 32 | default_nav_to_pose_bt_xml: will_be_handled_by_rewritten_yaml 33 | 34 | controller_server: 35 | ros__parameters: 36 | controller_frequency: 20.0 37 | failure_tolerance: 1.0 38 | min_x_velocity_threshold: 0.001 39 | min_y_velocity_threshold: 0.5 40 | min_theta_velocity_threshold: 0.001 41 | progress_checker_plugin: "progress_checker" 42 | goal_checker_plugins: ["general_goal_checker"] 43 | controller_plugins: ["GracefulController"] 44 | enable_stamped_cmd_vel: false 45 | 46 | # Progress checker parameters 47 | progress_checker: 48 | plugin: "nav2_controller::SimpleProgressChecker" 49 | required_movement_radius: 0.5 50 | movement_time_allowance: 10.0 51 | # Goal checker parameters 52 | general_goal_checker: 53 | plugin: "nav2_controller::SimpleGoalChecker" 54 | xy_goal_tolerance: 0.125 55 | yaw_goal_tolerance: 0.25 56 | stateful: True 57 | # Controller parameters 58 | GracefulController: 59 | plugin: "nav2_graceful_controller::GracefulController" 60 | v_linear_min: 0.1 61 | v_linear_max: 1.0 62 | v_angular_max: 2.8 63 | v_angular_min_in_place: 0.6 64 | max_lookahead: 1.25 65 | initial_rotation: true 66 | initial_rotation_tolerance: 0.25 67 | prefer_final_rotation: true 68 | # The defaults aren't as good as this (more wiggling) 69 | k_phi: 2.0 70 | k_delta: 1.0 71 | beta: 0.4 72 | lambda: 2.0 73 | # This isn't quite comparable to using actual acceleration limits 74 | # (but works well in practice) 75 | slowdown_radius: 0.75 76 | 77 | local_costmap: 78 | local_costmap: 79 | ros__parameters: 80 | update_frequency: 5.0 81 | publish_frequency: 2.0 82 | global_frame: odom 83 | robot_base_frame: base_link 84 | rolling_window: true 85 | width: 3 86 | height: 3 87 | resolution: 0.05 88 | robot_radius: 0.275 89 | footprint_padding: 0.025 90 | plugins: ["depth_layer", "inflation_layer"] 91 | inflation_layer: 92 | plugin: "nav2_costmap_2d::InflationLayer" 93 | cost_scaling_factor: 10.0 94 | inflation_radius: 0.75 95 | depth_layer: 96 | plugin: "nav2_costmap_2d::DepthLayer" 97 | publish_observations: True 98 | enabled: True 99 | publish_voxel_map: True 100 | origin_z: 0.0 101 | z_resolution: 0.125 102 | z_voxels: 16 103 | min_obstacle_height: 0.0 104 | max_obstacle_height: 2.0 105 | obstacle_min_range: 0.0 106 | obstacle_max_range: 3.0 107 | raytrace_min_range: 0.0 108 | raytrace_max_range: 4.0 109 | mark_threshold: 0 110 | observation_sources: base_scan 111 | base_scan: 112 | topic: /base_scan 113 | max_obstacle_height: 2.0 114 | clearing: True 115 | marking: True 116 | data_type: "LaserScan" 117 | always_send_full_costmap: True 118 | 119 | global_costmap: 120 | global_costmap: 121 | ros__parameters: 122 | update_frequency: 1.0 123 | publish_frequency: 1.0 124 | global_frame: map 125 | robot_base_frame: base_link 126 | robot_radius: 0.275 127 | footprint_padding: 0.025 128 | resolution: 0.05 129 | plugins: ["static_layer", "obstacle_layer", "inflation_layer"] 130 | obstacle_layer: 131 | plugin: "nav2_costmap_2d::ObstacleLayer" 132 | enabled: True 133 | observation_sources: scan 134 | scan: 135 | topic: /base_scan 136 | max_obstacle_height: 2.0 137 | clearing: True 138 | marking: True 139 | data_type: "LaserScan" 140 | static_layer: 141 | plugin: "nav2_costmap_2d::StaticLayer" 142 | map_subscribe_transient_local: True 143 | inflation_layer: 144 | plugin: "nav2_costmap_2d::InflationLayer" 145 | cost_scaling_factor: 10.0 146 | inflation_radius: 0.75 147 | always_send_full_costmap: True 148 | 149 | map_server: 150 | ros__parameters: 151 | yaml_filename: "map.yaml" 152 | 153 | planner_server: 154 | ros__parameters: 155 | expected_planner_frequency: 20.0 156 | planner_plugins: ["GridBased"] 157 | GridBased: 158 | plugin: "nav2_navfn_planner::NavfnPlanner" 159 | tolerance: 0.5 160 | use_astar: false 161 | allow_unknown: true 162 | 163 | behavior_server: 164 | ros__parameters: 165 | costmap_topic: local_costmap/costmap_raw 166 | footprint_topic: local_costmap/published_footprint 167 | cycle_frequency: 10.0 168 | behavior_plugins: ["spin", "backup", "wait"] 169 | spin: 170 | plugin: "nav2_behaviors::Spin" 171 | backup: 172 | plugin: "nav2_behaviors::BackUp" 173 | wait: 174 | plugin: "nav2_behaviors::Wait" 175 | global_frame: odom 176 | robot_base_frame: base_link 177 | transform_timeout: 0.1 178 | # Parameters for spin recovery 179 | simulate_ahead_time: 2.0 180 | max_rotational_vel: 2.8 181 | min_rotational_vel: 0.6 182 | rotational_acc_lim: 1.0 183 | # Temporary for Rolling/K-Turtle 184 | enable_stamped_cmd_vel: false 185 | -------------------------------------------------------------------------------- /ubr1_navigation/costmap_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A costmap layer that extracts ground plane and clears it. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /ubr1_navigation/include/ubr1_navigation/depth_layer.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020-2024, Michael Ferguson 3 | * Copyright (c) 2015-2016, Fetch Robotics Inc. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Fetch Robotics Inc. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL FETCH ROBOTICS INC. BE LIABLE FOR ANY 22 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 27 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | // Author: Anuj Pasricha, Michael Ferguson 31 | 32 | #ifndef UBR1_NAVIGATION__DEPTH_LAYER_HPP_ 33 | #define UBR1_NAVIGATION__DEPTH_LAYER_HPP_ 34 | 35 | #include 36 | 37 | #include "nav2_costmap_2d/observation_buffer.hpp" 38 | #include "nav2_costmap_2d/voxel_layer.hpp" 39 | #include "cv_bridge/cv_bridge.hpp" 40 | #include "image_transport/image_transport.hpp" 41 | #include "sensor_msgs/image_encodings.hpp" 42 | #include "sensor_msgs/msg/image.hpp" 43 | #include "sensor_msgs/msg/point_cloud2.hpp" 44 | #include "tf2_ros/message_filter.h" 45 | //#include 46 | 47 | #include 48 | using cv::rgbd::DepthCleaner; 49 | using cv::rgbd::RgbdNormals; 50 | using cv::rgbd::RgbdPlane; 51 | using cv::rgbd::depthTo3d; 52 | 53 | namespace nav2_costmap_2d 54 | { 55 | 56 | /** 57 | * @class DepthLayer 58 | * @brief A costmap layer that extracts ground plane and clears it. 59 | */ 60 | class DepthLayer : public VoxelLayer 61 | { 62 | public: 63 | /** 64 | * @brief Constructor 65 | */ 66 | DepthLayer(); 67 | 68 | /** 69 | * @brief Destructor for the depth costmap layer 70 | */ 71 | virtual ~DepthLayer(); 72 | 73 | /** 74 | * @brief Initialization function for the DepthLayer 75 | */ 76 | virtual void onInitialize(); 77 | 78 | private: 79 | void cameraInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr msg); 80 | void depthImageCallback(sensor_msgs::msg::Image::ConstSharedPtr msg); 81 | 82 | std::shared_ptr marking_buf_; 83 | std::shared_ptr clearing_buf_; 84 | 85 | // should we publish the marking/clearing observations 86 | bool publish_observations_; 87 | 88 | // distance away from ground plane at which 89 | // something is considered an obstacle 90 | double observations_threshold_; 91 | 92 | // if finding ground plane, limit the tilt 93 | // with respect to base_link frame 94 | double ground_threshold_; 95 | 96 | // should NANs be treated as +inf and used for clearing 97 | bool clear_nans_; 98 | 99 | // skipping of potentially noisy rays near the edge of the image 100 | int skip_rays_bottom_; 101 | int skip_rays_top_; 102 | int skip_rays_left_; 103 | int skip_rays_right_; 104 | 105 | // should skipped edge rays be used for clearing? 106 | bool clear_with_skipped_rays_; 107 | 108 | // retrieves depth image from head_camera 109 | // used to fit ground plane to 110 | std::shared_ptr> depth_image_sub_; 112 | std::shared_ptr> depth_image_filter_; 113 | 114 | // retrieves camera matrix for head_camera 115 | // used in calculating ground plane 116 | rclcpp::Subscription::SharedPtr camera_info_sub_; 117 | 118 | // publishes clearing observations 119 | rclcpp_lifecycle::LifecyclePublisher::SharedPtr clearing_pub_; 120 | 121 | // publishes marking observations 122 | rclcpp_lifecycle::LifecyclePublisher::SharedPtr marking_pub_; 123 | 124 | // camera intrinsics 125 | std::mutex mutex_K_; 126 | cv::Mat K_; 127 | 128 | // clean the depth image 129 | cv::Ptr depth_cleaner_; 130 | 131 | // depth image estimation 132 | cv::Ptr normals_estimator_; 133 | cv::Ptr plane_estimator_; 134 | }; 135 | 136 | } // namespace nav2_costmap_2d 137 | 138 | #endif // UBR1_NAVIGATION__DEPTH_LAYER_HPP 139 | -------------------------------------------------------------------------------- /ubr1_navigation/launch/build_map.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import (DeclareLaunchArgument, EmitEvent, LogInfo, 3 | RegisterEventHandler) 4 | from launch.events import matches_action 5 | from launch.substitutions import LaunchConfiguration 6 | from launch_ros.actions import LifecycleNode 7 | from launch_ros.event_handlers import OnStateTransition 8 | from launch_ros.events.lifecycle import ChangeState 9 | from lifecycle_msgs.msg import Transition 10 | from ament_index_python.packages import get_package_share_directory 11 | 12 | 13 | def generate_launch_description(): 14 | use_sim_time = LaunchConfiguration('use_sim_time') 15 | 16 | declare_use_sim_time_argument = DeclareLaunchArgument( 17 | 'use_sim_time', 18 | default_value='false', 19 | description='Use simulation/Gazebo clock') 20 | 21 | start_sync_slam_toolbox_node = LifecycleNode( 22 | parameters=[ 23 | get_package_share_directory("ubr1_navigation") + '/config/mapper_params_online_sync.yaml', 24 | { 25 | 'use_lifecycle_manager': False, 26 | 'use_sim_time': use_sim_time 27 | } 28 | ], 29 | package='slam_toolbox', 30 | executable='sync_slam_toolbox_node', 31 | name='slam_toolbox', 32 | namespace='', 33 | output='screen' 34 | ) 35 | 36 | configure_event = EmitEvent( 37 | event=ChangeState( 38 | lifecycle_node_matcher=matches_action(start_sync_slam_toolbox_node), 39 | transition_id=Transition.TRANSITION_CONFIGURE 40 | ) 41 | ) 42 | 43 | activate_event = RegisterEventHandler( 44 | OnStateTransition( 45 | target_lifecycle_node=start_sync_slam_toolbox_node, 46 | start_state="configuring", 47 | goal_state="inactive", 48 | entities=[ 49 | LogInfo(msg="[LifecycleLaunch] Slamtoolbox node is activating."), 50 | EmitEvent(event=ChangeState( 51 | lifecycle_node_matcher=matches_action(start_sync_slam_toolbox_node), 52 | transition_id=Transition.TRANSITION_ACTIVATE 53 | )) 54 | ] 55 | ) 56 | ) 57 | 58 | ld = LaunchDescription() 59 | 60 | ld.add_action(declare_use_sim_time_argument) 61 | ld.add_action(start_sync_slam_toolbox_node) 62 | ld.add_action(configure_event) 63 | ld.add_action(activate_event) 64 | 65 | return ld 66 | -------------------------------------------------------------------------------- /ubr1_navigation/launch/includes/move_base.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /ubr1_navigation/launch/localization.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | 19 | from launch import LaunchDescription 20 | from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable 21 | from launch.substitutions import LaunchConfiguration 22 | from launch_ros.actions import Node 23 | from nav2_common.launch import RewrittenYaml 24 | 25 | 26 | def generate_launch_description(): 27 | # Get the launch directory 28 | bringup_dir = get_package_share_directory('ubr1_navigation') 29 | 30 | namespace = LaunchConfiguration('namespace') 31 | map_yaml_file = LaunchConfiguration('map') 32 | use_sim_time = LaunchConfiguration('use_sim_time') 33 | autostart = LaunchConfiguration('autostart') 34 | params_file = LaunchConfiguration('params_file') 35 | lifecycle_nodes = ['map_server', 'amcl'] 36 | 37 | # Map fully qualified names to relative ones so the node's namespace can be prepended. 38 | # In case of the transforms (tf), currently, there doesn't seem to be a better alternative 39 | # https://github.com/ros/geometry2/issues/32 40 | # https://github.com/ros/robot_state_publisher/pull/30 41 | # TODO(orduno) Substitute with `PushNodeRemapping` 42 | # https://github.com/ros2/launch_ros/issues/56 43 | remappings = [('/tf', 'tf'), 44 | ('/tf_static', 'tf_static'), 45 | ('scan', 'base_scan')] 46 | 47 | # Create our own temporary YAML files that include substitutions 48 | param_substitutions = { 49 | 'use_sim_time': use_sim_time, 50 | 'yaml_filename': map_yaml_file} 51 | 52 | configured_params = RewrittenYaml( 53 | source_file=params_file, 54 | root_key=namespace, 55 | param_rewrites=param_substitutions, 56 | convert_types=True) 57 | 58 | return LaunchDescription([ 59 | # Set env var to print messages to stdout immediately 60 | SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), 61 | 62 | DeclareLaunchArgument( 63 | 'namespace', default_value='', 64 | description='Top-level namespace'), 65 | 66 | DeclareLaunchArgument( 67 | 'map', 68 | default_value=os.path.join(bringup_dir, 'maps', 'map_1728569604.yaml'), 69 | description='Full path to map yaml file to load'), 70 | 71 | DeclareLaunchArgument( 72 | 'use_sim_time', default_value='false', 73 | description='Use simulation (Gazebo) clock if true'), 74 | 75 | DeclareLaunchArgument( 76 | 'autostart', default_value='true', 77 | description='Automatically startup the nav2 stack'), 78 | 79 | DeclareLaunchArgument( 80 | 'params_file', 81 | default_value=os.path.join(bringup_dir, 'config', 'nav2_params.yaml'), 82 | description='Full path to the ROS2 parameters file to use'), 83 | 84 | Node( 85 | package='nav2_map_server', 86 | executable='map_server', 87 | name='map_server', 88 | output='screen', 89 | parameters=[configured_params], 90 | remappings=remappings), 91 | 92 | Node( 93 | package='nav2_amcl', 94 | executable='amcl', 95 | name='amcl', 96 | output='screen', 97 | parameters=[configured_params], 98 | remappings=remappings), 99 | 100 | Node( 101 | package='nav2_lifecycle_manager', 102 | executable='lifecycle_manager', 103 | name='lifecycle_manager_localization', 104 | output='screen', 105 | parameters=[{'use_sim_time': use_sim_time}, 106 | {'autostart': autostart}, 107 | {'node_names': lifecycle_nodes}]) 108 | ]) 109 | -------------------------------------------------------------------------------- /ubr1_navigation/launch/navigation.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | 19 | from launch import LaunchDescription 20 | from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable 21 | from launch.substitutions import LaunchConfiguration 22 | from launch_ros.actions import Node 23 | from nav2_common.launch import RewrittenYaml 24 | 25 | 26 | def generate_launch_description(): 27 | # Get the launch directory 28 | bringup_dir = get_package_share_directory('ubr1_navigation') 29 | 30 | namespace = LaunchConfiguration('namespace') 31 | use_sim_time = LaunchConfiguration('use_sim_time') 32 | autostart = LaunchConfiguration('autostart') 33 | params_file = LaunchConfiguration('params_file') 34 | default_nav_to_pose_bt_xml = LaunchConfiguration('default_nav_to_pose_bt_xml') 35 | map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local') 36 | 37 | lifecycle_nodes = ['controller_server', 38 | 'planner_server', 39 | 'behavior_server', 40 | 'bt_navigator'] 41 | 42 | # Map fully qualified names to relative ones so the node's namespace can be prepended. 43 | # In case of the transforms (tf), currently, there doesn't seem to be a better alternative 44 | # https://github.com/ros/geometry2/issues/32 45 | # https://github.com/ros/robot_state_publisher/pull/30 46 | # TODO(orduno) Substitute with `PushNodeRemapping` 47 | # https://github.com/ros2/launch_ros/issues/56 48 | remappings = [('/tf', 'tf'), 49 | ('/tf_static', 'tf_static')] 50 | 51 | # Create our own temporary YAML files that include substitutions 52 | param_substitutions = { 53 | 'use_sim_time': use_sim_time, 54 | 'autostart': autostart, 55 | 'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml, 56 | 'map_subscribe_transient_local': map_subscribe_transient_local} 57 | 58 | configured_params = RewrittenYaml( 59 | source_file=params_file, 60 | root_key=namespace, 61 | param_rewrites=param_substitutions, 62 | convert_types=True) 63 | 64 | return LaunchDescription([ 65 | # Set env var to print messages to stdout immediately 66 | SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), 67 | 68 | DeclareLaunchArgument( 69 | 'namespace', default_value='', 70 | description='Top-level namespace'), 71 | 72 | DeclareLaunchArgument( 73 | 'use_sim_time', default_value='false', 74 | description='Use simulation (Gazebo) clock if true'), 75 | 76 | DeclareLaunchArgument( 77 | 'autostart', default_value='true', 78 | description='Automatically startup the nav2 stack'), 79 | 80 | DeclareLaunchArgument( 81 | 'params_file', 82 | default_value=os.path.join(bringup_dir, 'config', 'nav2_params.yaml'), 83 | description='Full path to the ROS2 parameters file to use'), 84 | 85 | DeclareLaunchArgument( 86 | 'default_nav_to_pose_bt_xml', 87 | default_value=os.path.join(bringup_dir, 'behavior_trees', 'default.xml'), 88 | description='Full path to the behavior tree xml file to use'), 89 | 90 | DeclareLaunchArgument( 91 | 'map_subscribe_transient_local', default_value='true', 92 | description='Whether to set the map subscriber QoS to transient local'), 93 | 94 | Node( 95 | package='nav2_controller', 96 | executable='controller_server', 97 | output='screen', 98 | parameters=[configured_params], 99 | remappings=remappings), 100 | 101 | Node( 102 | package='nav2_planner', 103 | executable='planner_server', 104 | name='planner_server', 105 | output='screen', 106 | parameters=[configured_params], 107 | remappings=remappings), 108 | 109 | Node( 110 | package='nav2_behaviors', 111 | executable='behavior_server', 112 | name='behavior_server', 113 | output='screen', 114 | parameters=[configured_params], 115 | remappings=remappings), 116 | 117 | Node( 118 | package='nav2_bt_navigator', 119 | executable='bt_navigator', 120 | name='bt_navigator', 121 | output='screen', 122 | parameters=[configured_params], 123 | remappings=remappings), 124 | 125 | Node( 126 | package='nav2_lifecycle_manager', 127 | executable='lifecycle_manager', 128 | name='lifecycle_manager_navigation', 129 | output='screen', 130 | parameters=[{'use_sim_time': use_sim_time}, 131 | {'autostart': autostart}, 132 | {'node_names': lifecycle_nodes}]), 133 | 134 | Node( 135 | package='ubr1_navigation', 136 | executable='tilt_head.py', 137 | name='tilt_head_node', 138 | ), 139 | ]) 140 | 141 | -------------------------------------------------------------------------------- /ubr1_navigation/maps/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_navigation/maps/map.pgm -------------------------------------------------------------------------------- /ubr1_navigation/maps/map.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-5.1, -7.67, 0] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.196 8 | -------------------------------------------------------------------------------- /ubr1_navigation/maps/map_1728569604.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_navigation/maps/map_1728569604.pgm -------------------------------------------------------------------------------- /ubr1_navigation/maps/map_1728569604.yaml: -------------------------------------------------------------------------------- 1 | image: map_1728569604.pgm 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-1.12, -9.35, 0] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.196 8 | -------------------------------------------------------------------------------- /ubr1_navigation/maps/map_with_arena.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikeferguson/ubr_reloaded/d6afde6bfec400350d9793236190e17da3bb0053/ubr1_navigation/maps/map_with_arena.pgm -------------------------------------------------------------------------------- /ubr1_navigation/maps/map_with_arena.yaml: -------------------------------------------------------------------------------- 1 | image: map_with_arena.pgm 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-1.56, -1.33, 0] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.196 8 | -------------------------------------------------------------------------------- /ubr1_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ubr1_navigation 5 | 0.3.0 6 | Configuration and launch files for UBR-1 using ROS navigation 7 | Michael Ferguson 8 | BSD 9 | Michael Ferguson 10 | 11 | ament_cmake 12 | image_transport 13 | nav2_costmap_2d 14 | pluginlib 15 | sensor_msgs 16 | tf2_ros 17 | nav2_amcl 18 | nav2_behaviors 19 | nav2_bt_navigator 20 | nav2_controller 21 | nav2_lifecycle_manager 22 | nav2_map_server 23 | nav2_navfn_planner 24 | nav2_planner 25 | python3-transforms3d 26 | slam_toolbox 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /ubr1_navigation/srv/TiltControl.srv: -------------------------------------------------------------------------------- 1 | # Should the head tilt? 2 | bool enable_tilt 3 | # Should the head look in direction of plan? 4 | bool enable_look 5 | --- 6 | 7 | -------------------------------------------------------------------------------- /ubr_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ubr_msgs) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(builtin_interfaces REQUIRED) 6 | find_package(rosidl_default_generators REQUIRED) 7 | find_package(std_msgs REQUIRED) 8 | 9 | rosidl_generate_interfaces(${PROJECT_NAME} 10 | "msg/BoardInfo.msg" 11 | "msg/BreakerState.msg" 12 | "msg/BreakerInfo.msg" 13 | "msg/JointInfo.msg" 14 | "msg/MotorInfo.msg" 15 | "msg/RobotState.msg" 16 | "msg/DebugInfo.msg" 17 | "msg/ChargerInfo.msg" 18 | "srv/BreakerCommand.srv" 19 | DEPENDENCIES builtin_interfaces std_msgs 20 | ) 21 | 22 | ament_export_dependencies(rosidl_default_runtime) 23 | ament_package() 24 | -------------------------------------------------------------------------------- /ubr_msgs/msg/BoardInfo.msg: -------------------------------------------------------------------------------- 1 | string name 2 | uint64 stamp 3 | 4 | uint8 model_number 5 | uint16 hw_version 6 | uint16 sw_version 7 | 8 | uint8 boot_state 9 | 10 | float32 temperature 11 | 12 | uint32 packets_recv 13 | uint32 packets_bad 14 | uint32 packets_sent 15 | uint32 packets_back 16 | 17 | float64 last_recv 18 | float64 last_sent 19 | bool up_to_date 20 | -------------------------------------------------------------------------------- /ubr_msgs/msg/BreakerInfo.msg: -------------------------------------------------------------------------------- 1 | string name 2 | 3 | BreakerState state 4 | 5 | uint8 error 6 | uint8 enable_failed 7 | uint8 trip 8 | uint8 enable_cycle_count 9 | uint16 enable_lockout_cycles 10 | -------------------------------------------------------------------------------- /ubr_msgs/msg/BreakerState.msg: -------------------------------------------------------------------------------- 1 | # 2 | # Gives feedback on Breakers 3 | # 4 | 5 | uint8 STATE_NOPOWER = 0 6 | uint8 STATE_ENABLED = 1 7 | uint8 STATE_DISABLED = 2 8 | 9 | # The state of the breaker, should be one of the values seen above. 10 | uint8 state 11 | 12 | # The current flowing through the breaker, in Amps. 13 | float32 current 14 | 15 | # The temperature of the breaker, in degrees C. 16 | float32 temperature 17 | -------------------------------------------------------------------------------- /ubr_msgs/msg/ChargerInfo.msg: -------------------------------------------------------------------------------- 1 | uint64 stamp 2 | 3 | uint8 charger_state 4 | uint8 charger_error 5 | uint8 charger_error_state 6 | uint8 charger_last_error 7 | uint8 current_limit_cause 8 | uint8 battery_balance_state 9 | uint8 supply_input 10 | uint8 charging_mode 11 | uint16 charger_error_count 12 | 13 | float32 system_voltage 14 | float32 battery_bot_voltage 15 | float32 battery_top_voltage 16 | float32 dock_voltage 17 | float32 plug_voltage 18 | float32 charging_voltage 19 | float32 battery_temperature 20 | float32 charger_temperature 21 | float32 battery_current 22 | float32 charger_current 23 | float32 supply_current 24 | float32 system_current 25 | float32 charge_remaining 26 | float32 runtime_remaining 27 | float32 fan_speed 28 | 29 | float32 duty_a 30 | float32 duty_b 31 | 32 | string[] degug_names 33 | float32[] debug_values 34 | -------------------------------------------------------------------------------- /ubr_msgs/msg/DebugInfo.msg: -------------------------------------------------------------------------------- 1 | # This is a more detailed message for debugging 2 | RobotState info 3 | ChargerInfo charger 4 | JointInfo[] joints 5 | BoardInfo[] boards 6 | MotorInfo[] motors 7 | BreakerInfo[] breakers 8 | -------------------------------------------------------------------------------- /ubr_msgs/msg/JointInfo.msg: -------------------------------------------------------------------------------- 1 | string name 2 | float64 stamp 3 | 4 | uint8 MODE_DISABLED = 0 5 | uint8 MODE_EFFORT = 1 6 | uint8 MODE_VELOCITY = 2 7 | uint8 MODE_POSITION = 3 8 | 9 | uint8 mode 10 | 11 | float32 desired_position 12 | float32 desired_velocity 13 | float32 desired_effort 14 | 15 | float32 actual_position 16 | float32 actual_velocity 17 | float32 actual_effort 18 | 19 | float32 calibration_offset 20 | 21 | float32 position_lower_limit 22 | float32 position_upper_limit 23 | float32 velocity_limit 24 | float32 effort_limit 25 | -------------------------------------------------------------------------------- /ubr_msgs/msg/MotorInfo.msg: -------------------------------------------------------------------------------- 1 | string name 2 | uint64 stamp 3 | 4 | uint8 state 5 | 6 | float32 desired_velocity 7 | float32 desired_effort 8 | 9 | float32 velocity 10 | float32 effort 11 | float32 temperature 12 | 13 | float32 velocity_limit 14 | float32 effort_limit 15 | float32 power_limit 16 | -------------------------------------------------------------------------------- /ubr_msgs/msg/RobotState.msg: -------------------------------------------------------------------------------- 1 | # 2 | # Primary message for users to get 3 | # information about the robot 4 | # 5 | std_msgs/Header header 6 | 7 | # The supply voltage of the robot. 8 | float32 supply_voltage 9 | 10 | # The estimated battery level, 0.0 to 1.0 11 | float32 battery_level 12 | 13 | # True if the robot is currently charging from the plug or the dock. 14 | bool is_charging 15 | 16 | # True if the robot is plugged in. 17 | bool plugged_in 18 | 19 | # True if the robot is on the dock. 20 | bool on_dock 21 | 22 | # True if the physical runstop on the robot is triggered. 23 | bool run_stopped 24 | 25 | # True if the software runstop has been triggered. 26 | bool run_stopped_in_sw 27 | 28 | # True if we have a hardware fault 29 | bool hardware_fault 30 | 31 | # State of the motor breaker 32 | BreakerState motor_breaker_state 33 | -------------------------------------------------------------------------------- /ubr_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ubr_msgs 5 | 0.3.0 6 | 7 | Messages and Services definitions for Unbounded Robotics. 8 | 9 | Michael Ferguson 10 | Michael Ferguson 11 | BSD 12 | http://ros.org/wiki/ubr_msgs 13 | 14 | ament_cmake 15 | rosidl_default_generators 16 | 17 | builtin_interfaces 18 | rosidl_default_generators 19 | std_msgs 20 | 21 | rosidl_default_runtime 22 | 23 | rosidl_interface_packages 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /ubr_msgs/srv/BreakerCommand.srv: -------------------------------------------------------------------------------- 1 | # 2 | # Service for enabling/disabling breakers 3 | # 4 | bool enable 5 | --- 6 | BreakerState status 7 | -------------------------------------------------------------------------------- /ubr_teleop/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ubr_teleop) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 14) 6 | endif() 7 | 8 | find_package(ament_cmake REQUIRED) 9 | find_package(control_msgs REQUIRED) 10 | find_package(geometry_msgs REQUIRED) 11 | find_package(rclcpp REQUIRED) 12 | find_package(rclcpp_action REQUIRED) 13 | find_package(sensor_msgs REQUIRED) 14 | 15 | include_directories(include) 16 | 17 | add_executable(keyboard_teleop src/keyboard_teleop.cpp) 18 | ament_target_dependencies(keyboard_teleop 19 | control_msgs 20 | geometry_msgs 21 | rclcpp 22 | rclcpp_action 23 | sensor_msgs 24 | ) 25 | 26 | add_executable(joystick_teleop src/joystick_teleop.cpp) 27 | ament_target_dependencies(joystick_teleop 28 | control_msgs 29 | geometry_msgs 30 | rclcpp 31 | rclcpp_action 32 | sensor_msgs 33 | ) 34 | 35 | install( 36 | TARGETS 37 | keyboard_teleop 38 | joystick_teleop 39 | ARCHIVE DESTINATION lib 40 | LIBRARY DESTINATION lib 41 | RUNTIME DESTINATION lib/${PROJECT_NAME} 42 | ) 43 | 44 | install( 45 | PROGRAMS 46 | scripts/set_gripper_pose.py 47 | scripts/set_head_pose.py 48 | scripts/set_torso_pose.py 49 | scripts/tuck_arm.py 50 | DESTINATION 51 | lib/${PROJECT_NAME} 52 | ) 53 | 54 | ament_export_include_directories(include) 55 | ament_export_dependencies( 56 | control_msgs 57 | geometry_msgs 58 | rclcpp 59 | rclcpp_action 60 | sensor_msgs 61 | ) 62 | ament_package() 63 | 64 | -------------------------------------------------------------------------------- /ubr_teleop/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ubr_teleop 5 | 0.3.0 6 | 7 | Teleoperation for the robot. 8 | 9 | Michael Ferguson 10 | Michael Ferguson 11 | BSD 12 | http://ros.org/wiki/ubr_teleop 13 | 14 | ament_cmake 15 | 16 | control_msgs 17 | geometry_msgs 18 | rclcpp 19 | rclcpp_action 20 | sensor_msgs 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /ubr_teleop/scripts/set_gripper_pose.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | # Copryight (c) 2020 Michael Ferguson 4 | # Copyright (c) 2013-2014 Unbounded Robotics Inc. 5 | # All right reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above copyright 13 | # notice, this list of conditions and the following disclaimer in the 14 | # documentation and/or other materials provided with the distribution. 15 | # * Neither the name of Unbounded Robotics Inc. nor the names of its 16 | # contributors may be used to endorse or promote products derived 17 | # from this software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | # DISCLAIMED. IN NO EVENT SHALL UNBOUNDED ROBOTICS INC. BE LIABLE FOR ANY DIRECT, INDIRECT, 23 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 24 | # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 25 | # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 26 | # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 28 | # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | """ 31 | Usage: set_gripper_pose.py 32 | is the opening of the gripper, or 'open' or 'closed' for predefined positions 33 | is the max effort to use. 34 | """ 35 | 36 | import sys 37 | 38 | import rclpy 39 | from rclpy.node import Node 40 | from rclpy.duration import Duration 41 | from rclpy.action import ActionClient 42 | 43 | from control_msgs.action import GripperCommand 44 | 45 | 46 | class GripperClient(Node): 47 | 48 | def __init__(self): 49 | super().__init__("set_gripper_pose") 50 | self._action_client = ActionClient(self, GripperCommand, 'gripper_controller/command') 51 | 52 | def run(self, position, max_effort): 53 | goal = GripperCommand.Goal() 54 | goal.command.position = position 55 | goal.command.max_effort = max_effort 56 | self._action_client.wait_for_server() 57 | self._action_client.send_goal_async(goal) 58 | 59 | 60 | if __name__ == "__main__": 61 | 62 | if len(sys.argv) < 2: 63 | print(__doc__) 64 | exit(-1) 65 | else: 66 | position = 0.0 67 | max_effort = 28.0 68 | if len(sys.argv) == 3: 69 | max_effort = float(sys.argv[2]) 70 | if sys.argv[1] == "open": 71 | position = 0.09 72 | elif sys.argv[1] == "closed": 73 | position = 0.0 74 | else: 75 | position = float(sys.argv[1]) 76 | 77 | rclpy.init() 78 | action_client = GripperClient() 79 | action_client.run(position, max_effort) 80 | -------------------------------------------------------------------------------- /ubr_teleop/scripts/set_head_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copryight (c) 2020 Michael Ferguson 4 | # Copyright (c) 2013-2014 Unbounded Robotics Inc. 5 | # All right reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above copyright 13 | # notice, this list of conditions and the following disclaimer in the 14 | # documentation and/or other materials provided with the distribution. 15 | # * Neither the name of Unbounded Robotics Inc. nor the names of its 16 | # contributors may be used to endorse or promote products derived 17 | # from this software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | # DISCLAIMED. IN NO EVENT SHALL UNBOUNDED ROBOTICS INC. BE LIABLE FOR ANY DIRECT, INDIRECT, 23 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 24 | # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 25 | # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 26 | # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 28 | # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | """ 31 | Usage: set_head_pose.py 32 | is the head pan location (-1.57 to 1.57 radians) 33 | is the head tilt location (-1.57 to 0.785 radians) 34 | """ 35 | 36 | import sys 37 | 38 | import rclpy 39 | from rclpy.node import Node 40 | from rclpy.duration import Duration 41 | from rclpy.action import ActionClient 42 | 43 | from control_msgs.action import FollowJointTrajectory 44 | from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint 45 | 46 | 47 | class FollowJointTrajectoryClient(Node): 48 | 49 | def __init__(self): 50 | super().__init__("set_head_pose") 51 | self._action_client = ActionClient(self, FollowJointTrajectory, 'head_controller/follow_joint_trajectory') 52 | 53 | def run(self, trajectory): 54 | goal = FollowJointTrajectory.Goal() 55 | goal.trajectory = trajectory 56 | goal.goal_time_tolerance = Duration(seconds=0).to_msg() 57 | self._action_client.wait_for_server() 58 | self._action_client.send_goal_async(goal) 59 | 60 | 61 | if __name__=='__main__': 62 | if len(sys.argv) < 3: 63 | print(__doc__) 64 | exit(-1) 65 | 66 | trajectory = JointTrajectory() 67 | trajectory.joint_names = ['head_pan_joint', 'head_tilt_joint'] 68 | trajectory.points.append(JointTrajectoryPoint()) 69 | trajectory.points[0].positions = [float(sys.argv[1]), float(sys.argv[2])] 70 | trajectory.points[0].velocities = [0.0, 0.0] 71 | trajectory.points[0].time_from_start = Duration(seconds=1).to_msg() 72 | 73 | rclpy.init() 74 | action_client = FollowJointTrajectoryClient() 75 | action_client.run(trajectory) 76 | -------------------------------------------------------------------------------- /ubr_teleop/scripts/set_torso_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copryight (c) 2020 Michael Ferguson 4 | # Copyright (c) 2013-2014 Unbounded Robotics Inc. 5 | # All right reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above copyright 13 | # notice, this list of conditions and the following disclaimer in the 14 | # documentation and/or other materials provided with the distribution. 15 | # * Neither the name of Unbounded Robotics Inc. nor the names of its 16 | # contributors may be used to endorse or promote products derived 17 | # from this software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | # DISCLAIMED. IN NO EVENT SHALL UNBOUNDED ROBOTICS INC. BE LIABLE FOR ANY DIRECT, INDIRECT, 23 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 24 | # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 25 | # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 26 | # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 28 | # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | """ 31 | Usage: set_torso_pose.py 32 | is the height of the torso (0.0 to 0.35m) 33 | """ 34 | 35 | import sys 36 | 37 | import rclpy 38 | from rclpy.node import Node 39 | from rclpy.duration import Duration 40 | from rclpy.action import ActionClient 41 | 42 | from control_msgs.action import FollowJointTrajectory 43 | from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint 44 | 45 | 46 | class FollowJointTrajectoryClient(Node): 47 | 48 | def __init__(self): 49 | super().__init__("set_torso_pose") 50 | self._action_client = ActionClient(self, FollowJointTrajectory, 'torso_controller/follow_joint_trajectory') 51 | 52 | def run(self, trajectory): 53 | goal = FollowJointTrajectory.Goal() 54 | goal.trajectory = trajectory 55 | goal.goal_time_tolerance = Duration(seconds=0).to_msg() 56 | self._action_client.wait_for_server() 57 | self._action_client.send_goal_async(goal) 58 | 59 | 60 | if __name__=='__main__': 61 | if len(sys.argv) < 2: 62 | print(__doc__) 63 | exit(-1) 64 | 65 | trajectory = JointTrajectory() 66 | trajectory.joint_names = ["torso_lift_joint"] 67 | trajectory.points.append(JointTrajectoryPoint()) 68 | trajectory.points[0].positions = [float(sys.argv[1])] 69 | trajectory.points[0].velocities = [0.0] 70 | trajectory.points[0].accelerations = [0.0] 71 | trajectory.points[0].time_from_start = Duration(seconds=7).to_msg() # we can get anywhere by 7s 72 | 73 | rclpy.init() 74 | action_client = FollowJointTrajectoryClient() 75 | action_client.run(trajectory) 76 | -------------------------------------------------------------------------------- /ubr_teleop/scripts/tuck_arm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copryight (c) 2020 Michael Ferguson 4 | # Copyright (c) 2013-2014 Unbounded Robotics Inc. 5 | # All right reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above copyright 13 | # notice, this list of conditions and the following disclaimer in the 14 | # documentation and/or other materials provided with the distribution. 15 | # * Neither the name of Unbounded Robotics Inc. nor the names of its 16 | # contributors may be used to endorse or promote products derived 17 | # from this software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | # DISCLAIMED. IN NO EVENT SHALL UNBOUNDED ROBOTICS INC. BE LIABLE FOR ANY DIRECT, INDIRECT, 23 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 24 | # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 25 | # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 26 | # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 28 | # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | """ 31 | Usage: tuck_arm.py 32 | Tucks the robot arm (does not perform collision avoidance) 33 | """ 34 | 35 | import sys 36 | 37 | import rclpy 38 | from rclpy.node import Node 39 | from rclpy.duration import Duration 40 | from rclpy.action import ActionClient 41 | 42 | from control_msgs.action import FollowJointTrajectory 43 | from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint 44 | 45 | 46 | class TuckArm(Node): 47 | 48 | joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", 49 | "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] 50 | tucked = [-1.3901, 1.3439, -2.8327, -1.8119, 0.0, -1.6571, 0.0] 51 | 52 | def __init__(self): 53 | super().__init__("tuck_arm") 54 | self._action_client = ActionClient(self, FollowJointTrajectory, 'arm_controller/follow_joint_trajectory') 55 | 56 | def tuck(self): 57 | goal = FollowJointTrajectory.Goal() 58 | trajectory = JointTrajectory() 59 | trajectory.joint_names = self.joint_names 60 | trajectory.points.append(JointTrajectoryPoint()) 61 | trajectory.points[0].positions = self.tucked 62 | trajectory.points[0].velocities = [0.0 for i in self.joint_names] 63 | trajectory.points[0].accelerations = [0.0 for i in self.joint_names] 64 | trajectory.points[0].time_from_start = Duration(seconds=5).to_msg() 65 | goal.trajectory = trajectory 66 | goal.goal_time_tolerance = Duration(seconds=0).to_msg() 67 | self._action_client.wait_for_server() 68 | self._action_client.send_goal_async(goal) 69 | 70 | 71 | if __name__=='__main__': 72 | rclpy.init() 73 | action_client = TuckArm() 74 | action_client.tuck() 75 | -------------------------------------------------------------------------------- /ubr_teleop/src/keyboard_teleop.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Derived a bit from teleop_pr2_keyboard 5 | * Copyright (c) 2013-2014, Unbounded Robotics Inc. 6 | * Copyright (c) 2008, Willow Garage, Inc. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of Unbounded Robotics nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | *********************************************************************/ 36 | 37 | // Author: Michael Ferguson, Kevin Watts 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | 47 | #define KEYCODE_A 0x61 48 | #define KEYCODE_D 0x64 49 | #define KEYCODE_S 0x73 50 | #define KEYCODE_W 0x77 51 | #define KEYCODE_Q 0x71 52 | #define KEYCODE_E 0x65 53 | 54 | #define KEYCODE_A_CAP 0x41 55 | #define KEYCODE_D_CAP 0x44 56 | #define KEYCODE_S_CAP 0x53 57 | #define KEYCODE_W_CAP 0x57 58 | #define KEYCODE_Q_CAP 0x51 59 | #define KEYCODE_E_CAP 0x45 60 | 61 | #define KEYCODE_RIGHT 0x43 62 | #define KEYCODE_LEFT 0x44 63 | #define KEYCODE_UP 0x41 64 | #define KEYCODE_DOWN 0x42 65 | 66 | int kfd = 0; 67 | struct termios cooked, raw; 68 | std::shared_ptr robot; 69 | rclcpp::Time last; 70 | 71 | void quit(int sig) 72 | { 73 | robot->stop(); 74 | tcsetattr(kfd, TCSANOW, &cooked); 75 | rclcpp::shutdown(); 76 | exit(0); 77 | } 78 | 79 | void callback() 80 | { 81 | if ((robot->now() - last) > rclcpp::Duration(0, 5e8)) 82 | robot->stop(); 83 | robot->sendCommands(); 84 | } 85 | 86 | int main(int argc, char ** argv) 87 | { 88 | rclcpp::init(argc, argv); 89 | 90 | robot.reset(new RobotController("keyboard_teleop")); 91 | robot->start(); 92 | 93 | last = robot->now(); 94 | rclcpp::TimerBase::SharedPtr timer = 95 | robot->create_wall_timer(std::chrono::milliseconds(50), callback); 96 | 97 | //ros::AsyncSpinner spinner(1); 98 | //spinner.start(); 99 | 100 | signal(SIGINT, quit); 101 | 102 | char c; 103 | bool dirty=false; 104 | 105 | // get the console in raw mode 106 | tcgetattr(kfd, &cooked); 107 | memcpy(&raw, &cooked, sizeof(struct termios)); 108 | raw.c_lflag &=~ (ICANON | ECHO); 109 | // Setting a new line, then end of file 110 | raw.c_cc[VEOL] = 1; 111 | raw.c_cc[VEOF] = 2; 112 | tcsetattr(kfd, TCSANOW, &raw); 113 | 114 | puts("Reading from keyboard"); 115 | puts("---------------------------"); 116 | puts("Use 'WASD' to translate"); 117 | puts("Use arrow keys to move head"); 118 | puts("Any other key to stop"); 119 | 120 | while (rclcpp::ok()) 121 | { 122 | // get the next event from the keyboard 123 | if (read(kfd, &c, 1) < 0) 124 | { 125 | perror("read():"); 126 | exit(-1); 127 | } 128 | 129 | last = robot->now(); 130 | robot->start(); 131 | 132 | switch (c) 133 | { 134 | 135 | case KEYCODE_W: 136 | robot->setBaseVelocity(0.5, 0); 137 | break; 138 | case KEYCODE_S: 139 | robot->setBaseVelocity(-0.5, 0); 140 | break; 141 | case KEYCODE_A: 142 | robot->setBaseVelocity(0, 1.0); 143 | break; 144 | case KEYCODE_D: 145 | robot->setBaseVelocity(0, -1.0); 146 | break; 147 | 148 | case KEYCODE_RIGHT: 149 | robot->setHeadPosition(robot->getHeadPan() - 0.1, robot->getHeadTilt()); 150 | break; 151 | case KEYCODE_LEFT: 152 | robot->setHeadPosition(robot->getHeadPan() + 0.1, robot->getHeadTilt()); 153 | break; 154 | 155 | case KEYCODE_DOWN: 156 | robot->setHeadPosition(robot->getHeadPan(), robot->getHeadTilt() - 0.1); 157 | break; 158 | case KEYCODE_UP: 159 | robot->setHeadPosition(robot->getHeadPan(), robot->getHeadTilt() + 0.1); 160 | break; 161 | 162 | default: 163 | robot->setBaseVelocity(0, 0); 164 | } 165 | } 166 | 167 | return 0; 168 | } 169 | --------------------------------------------------------------------------------