Merge pull request #32 from ethz-asl/integration

Integration

.gitignore

@@ -11,5 +11,12 @@ venv
 # Vim stuff
 *.swp
 
+# Ansible stuff
+*.retry
 
+# Catkin stuff
 CATKIN_IGNORE
+
+# Files that will be auto-generated on compile
+moma_demos/grasp_demo/config/yumi/gpd.cfg
+moma_demos/grasp_demo/config/panda/gpd.cfg

.gitmodules

@@ -9,3 +9,13 @@
 [submodule "realsense-ros"]
 	path = submodules/realsense-ros
 	url = [email protected]:IntelRealSense/realsense-ros.git
+[submodule "submodules/gpd"]
+	path = submodules/gpd
+	url = https://github.com/kekeblom/gpd
+[submodule "submodules/gpd_ros"]
+	path = submodules/gpd_ros
+	url = ssh://[email protected]/kekeblom/gpd_ros
+[submodule "submodules/yumi"]
+	path = submodules/yumi
+	url = [email protected]:ethz-asl/yumi.git
+	branch = asl-devel

README.md

@@ -31,6 +31,10 @@ Then, use catkin to build the desired packages.
 
 Before you start developing, familiarize yourself with the [robotic platform](https://github.com/ethz-asl/moma/wiki/Robots) you will be working with and make sure to checkout the [Development](https://github.com/ethz-asl/moma/wiki/Development) section of our wiki.
 
+## GPD
+
+When building a package that relies on GPD (e.g. `fetch_demo`), GPD needs to be built separately first. For the installation instructions, refer to `submodules/gpd/README.md`.
+
 ## Documentation
 
 Every package should contain a `README.md` file documenting its main features.
@@ -39,18 +43,15 @@ More detailed instructions on the setup and operation of our robotic platforms,
 
 ## Ansible Playbooks
 
-This section needs to be updated.
-
-The `playbooks/` directory contains some Ansible playbooks which are there to automate and document how to set up the machine. `playbooks/setup_ros.yaml` will install ROS. `playbooks/setup_repo.yaml` will create a catkin workspace and clone this repository on the machine. `playbooks/setup_realsense.yaml` will install realsense dependencies and clone the [realsense-ros](https://github.com/IntelRealSense/realsense-ros) repository.
+The `operations/` directory contains some Ansible playbooks which are there to automate and document how to set up a computer.
 
-To run a playbook, first install Ansible (`pip install --user ansible`). Copy the file `playbooks/hosts` to `/opt/ansible/hosts` or specify the inventory file with the `-i` option every time you run `ansible-playbook` (e.g. `ansible-playbook playbooks/setup_ros.yaml -i playbooks/hosts --ask-pass --ask-become-pass`.
+To run a playbook, first install Ansible (`pip install --user ansible`). Copy the hosts file `operations/hosts` to `/etc/ansible/hosts`.
 
-The hosts file is an inventory file specifying which machines the playbooks are run against. Currently it only contains one group `franka-box` which's ip should point to the ubuntu machine controlling the Franka arm.
-
-Setting up the machine is done by running the commands:
+The hosts file is an inventory file specifying groups of hosts. A playbook is always run against a group and all the commands will be run on all machines in that group.
 
+To set up the franka control computer, run:
 ```
-ansible-playbook playbooks/setup_ros.yaml --ask-pass --ask-become-pass
-ansible-playbook playbooks/setup_repo.yaml --ask-pass --ask-become-pass
-ansible-playbook playbooks/setup_realsense.yaml --ask-pass --ask-become-pass
+ansible-playbook operations/setup_franka.yaml --ask-pass --ask-become-pass
 ```
+The `--ask-pass` flag is not needed if ssh authentication is being used.
+

install_dependencies.sh

@@ -4,5 +4,11 @@ sudo apt-get install ros-$ROS_DISTRO-ridgeback-description \
 	ros-$ROS_DISTRO-moveit-simple-controller-manager \
 	ros-$ROS_DISTRO-ros-controllers \
 	ros-$ROS_DISTRO-ridgeback-simulator \
-	ros-$ROS_DISTRO-ridgeback-desktop
-
+	ros-$ROS_DISTRO-ridgeback-desktop \
+	ros-$ROS_DISTRO-moveit \
+	ros-$ROS_DISTRO-rosmon \
+	ros-$ROS_DISTRO-pcl-ros \
+	ros-$ROS_DISTRO-tf2-sensor-msgs \
+	ros-$ROS_DISTRO-rqt-py-trees \
+	ros-$ROS_DISTRO-libfranka \
+	qtbase5-dev

moma_bringup/launch/mobmi_control_moveit_rviz.launch

@@ -0,0 +1,28 @@
+<?xml version="1.0" ?>
+<launch>
+
+  <arg name="launch_rviz" default="true"/>
+
+  <!-- Load robot description -->
+  <param name="robot_description" command="$(find xacro)/xacro '$(find moma_description)/urdf/mobmi.urdf.xacro'" />
+
+  <!-- Launch Yumi drivers -->
+  <include file="$(find yumi_support)/launch/robot_interface.launch"/>
+
+  <!-- Launch MoveIt -->
+  <include file="$(find mobmi_moveit_config)/launch/move_group.launch">
+    <arg name="load_robot_description" value="false" />
+    <arg name="allow_trajectory_execution" value="true" />
+    <arg name="fake_execution" value="false"/>
+    <arg name="info" value="true"/>
+  </include>
+
+  <!-- Launch state publisher -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen"/>
+
+  <!-- Launch rviz if requested -->
+  <include if="$(arg launch_rviz)" file="$(find mobmi_moveit_config)/launch/moveit_rviz.launch">
+    <arg name="rviz_config" value="$(find mobmi_moveit_config)/launch/moveit.rviz" />
+  </include>
+
+</launch>

moma_bringup/launch/panda_control_moveit_rviz.launch

@@ -1,11 +1,11 @@
 <?xml version="1.0" ?>
 <launch>
-  <!-- Adapted from $(find panda_moveit_config)/launch/panda_control_moveit_rviz.launch -->
-
   <arg name="launch_rviz" default="true" />
 
   <!-- Launch the panda control node -->
-  <include file="$(find panda_control)/launch/panda_control.launch"/>
+  <include file="$(find panda_control)/launch/panda_control.launch">
+    <arg name="robot_description" value="$(find moma_description)/urdf/panda.urdf.xacro"/>
+  </include>
 
   <!-- Start a joint trajectory controller -->
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="position_joint_trajectory_controller"/>

moma_bringup/launch/royalpanda_control_moveit_rviz.launch

@@ -0,0 +1,20 @@
+<?xml version="1.0" ?>
+<launch>
+  <arg name="launch_rviz" default="true" />
+
+  <!-- Launch the panda control node -->
+  <include file="$(find panda_control)/launch/panda_control.launch">
+    <arg name="robot_description" value="$(find moma_description)/urdf/mopa.urdf.xacro"/>
+  </include>
+
+  <!-- Start a joint trajectory controller -->
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="position_joint_trajectory_controller"/>
+
+  <!-- Launch MoveIt -->
+  <include file="$(find panda_moveit_config)/launch/move_group.launch"/>
+
+  <!-- If desired, launch a preconfigured Rviz -->
+  <include if="$(arg launch_rviz)" file="$(find panda_moveit_config)/launch/moveit_rviz.launch">
+    <arg name="rviz_config" value="$(find panda_moveit_config)/launch/moveit.rviz" />
+  </include>
+</launch>

moma_bringup/launch/yumi_control_moveit_rviz.launch

@@ -1,14 +1,18 @@
 <?xml version="1.0" ?>
 <launch>
 
-  <arg name="rviz" default="true"/>
+  <arg name="launch_rviz" default="true"/>
+
+  <!-- Load robot description -->
+  <param name="robot_description" command="$(find xacro)/xacro '$(find moma_description)/urdf/yumi.urdf.xacro'" />
 
   <!-- Launch Yumi drivers -->
   <include file="$(find yumi_support)/launch/robot_interface.launch"/>
 
   <!-- Launch MoveIt -->
   <include file="$(find yumi_moveit_config)/launch/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>
+    <arg name="load_robot_description" value="false" />
+    <arg name="allow_trajectory_execution" value="true" />
     <arg name="fake_execution" value="false"/>
     <arg name="info" value="true"/>
   </include>
@@ -17,7 +21,7 @@
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen"/>
 
   <!-- Launch rviz if requested -->
-  <include if="$(arg rviz)" file="$(find yumi_moveit_config)/launch/moveit_rviz.launch">
+  <include if="$(arg launch_rviz)" file="$(find yumi_moveit_config)/launch/moveit_rviz.launch">
     <arg name="rviz_config" value="$(find yumi_moveit_config)/launch/moveit.rviz" />
   </include>
 

moma_demos/fetch_demo/README.md

@@ -3,34 +3,26 @@
 
 ## Launching the demo on the multi-machine setup
 
-### As a single launch file
-
-On the machine used for the user interface, run the following command to distribute the nodes.
-
-```bash
-roslaunch fetch_demo single_command.launch
-```
-
 ### Via SSH
 
 There is one launch file for each machine in the setup. They can all be launch from the user interface laptop via SSH as follows.
 
 Navigation nodes on ridgeback machine:
 
 ```bash
-ssh asl-admin@asl-ridgeback "source /home/asl-admin/catkin_ws/devel/setup.zsh && roslaunch fetch_demo navigation.launch"
+ssh -t asl-admin@asl-ridgeback "source /home/asl-admin/.zshrc && roslaunch fetch_demo navigation.launch"
 ```
 
 Manipulation nodes on YuMi machine:
 
 ```bash
-ssh asl-admin@asl-yumi "source /home/asl-admin/catkin_ws/devel/setup.zsh && roslaunch fetch_demo manipulation.launch"
+ssh -t asl-admin@asl-yumi "source /home/asl-admin/.zshrc && roslaunch fetch_demo manipulation.launch"
 ```
 
 Perception nodes on moma-laptop:
 
 ```bash
-ssh asl-admin@moma-laptop "source /home/asl-admin/catkin_ws/devel/setup.zsh && roslaunch fetch_demo perception.launch"
+ssh -t asl-admin@moma-laptop "source /home/asl-admin/.zshrc && roslaunch fetch_demo perception.launch"
 ```
 
 On local user interface machine:
@@ -40,3 +32,20 @@ roslaunch fetch_demo user_interface.launch
 ```
 
 Each of these commands is blocking, they should be run in separate terminals.
+
+
+## Useful commands
+
+### Test the approach node
+
+Send goals directly to move_base:
+
+``bash
+rosrun actionlib axclient.py move_base move_base_msgs/MoveBaseAction
+```
+
+Send goals to the approach action:
+
+```bash
+rosrun actionlib axclient.py approach_action fetch_demo/ApproachAction
+```

moma_demos/fetch_demo/config/fetch_demo.yaml

@@ -0,0 +1,23 @@
+# General settings:
+arm_velocity_scaling_search: 0.5
+
+# Waypoints for the base to follow during search action:
+search_waypoints:
+  initial:
+    - [15.6757316589, -0.985509037971, -1.14762610015]
+    - [18.4454135895, -1.62419974804, -0.293683380035]
+    - [21.1538028717, -1.79279625416, 6.57794558821]
+
+  loop:
+    - [22.884, -0.478, 63.0]
+    - [23.3571300507, 0.906202673912, 91.400792341]
+    - [23.2464008331, 2.67486047745, 89.8622512493]
+    - [23.2799835205, 4.79860496521, 107.616658638]
+
+drop_waypoints:
+  retract_waypoint: [22.6979408264, -0.706754267216, -136.143811141]
+  drop_waypoints: 
+    - [18.8157615662, -1.54435634613, 174.374674869]
+    - [16.5263442993, -1.03566563129, -179.855710433]
+    - [10.8428039551, -1.36035263538, 178.827247868]
+    - [4.33228302002, -1.1852132082, 178.782756016]