MoveIt integration on frankapy

.gitignore

@@ -26,6 +26,7 @@ share/python-wheels/
 .installed.cfg
 *.egg
 MANIFEST
+.vscode/
 
 # PyInstaller
 #  Usually these files are written by a python script from a template

Dockerfile

@@ -0,0 +1,42 @@
+FROM osrf/ros:noetic-desktop-full
+
+# Install apt packages
+RUN apt update && apt install -y git nano
+RUN apt install ros-noetic-tf2-tools -y
+RUN apt install python3-pip python3-tk -y
+
+# install python dependencies to run frankapy within the docker container
+RUN pip3 install autolab_core 
+RUN pip3 install --force-reinstall pillow==9.0.1 && pip3 install --force-reinstall scipy==1.8
+RUN pip3 install numpy-quaternion numba && pip3 install --upgrade google-api-python-client 
+RUN pip3 install --force-reinstall numpy==1.23.5
+
+# Install moveit and franka_ros
+RUN apt install ros-noetic-moveit ros-noetic-franka-ros  -y
+
+# Make src/git_packages and clone panda_moveit_config
+RUN mkdir -p /home/ros_ws/src/git_packages && \
+    cd /home/ros_ws/src/git_packages && \
+    git clone https://github.com/ros-planning/panda_moveit_config.git -b noetic-devel
+
+# Copy the frankapy folder into the container
+COPY frankapy /home/ros_ws/src/git_packages/frankapy/frankapy
+COPY catkin_ws /home/ros_ws/src/git_packages/frankapy/catkin_ws
+
+# Copy src folder from desktop to /home/ros_ws/src/devel_packages (this is where we will put our custom packages)
+COPY moveit_frankapy/src/devel_packages /home/ros_ws/src/devel_packages
+
+# Install dependencies using rosdep
+RUN /bin/bash -c "source /opt/ros/noetic/setup.bash; cd /home/ros_ws; rosdep install --from-paths src --ignore-src -r -y"
+
+# add ros workspace to bashrc
+RUN echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
+RUN echo "source /home/ros_ws/devel/setup.bash" >> ~/.bashrc
+
+# build workspace
+RUN /bin/bash -c "source /opt/ros/noetic/setup.bash; cd /home/ros_ws; catkin_make"
+
+# set workdir as home/ros_ws
+WORKDIR /home/ros_ws
+
+CMD [ "bash" ]

moveit_frankapy/.gitignore

@@ -0,0 +1,32 @@
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app

moveit_frankapy/README.md

@@ -0,0 +1,46 @@
+# moveit_frankapy
+
+Prerequisites:
+
+- Install Docker: https://docs.docker.com/engine/install/ubuntu/
+
+- (Optional) Add docker to sudoers: https://docs.docker.com/engine/install/linux-postinstall/. If not done, please run all docker commands with a ```sudo```
+
+
+<br>
+
+Steps to run moveit with frankapy
+Frankapy Setup
+- Clone repo: git clone --recursive  https://github.com/vib2810/frankapy.git
+- bash start_control_pc.sh -u student -i iam-snowwhite
+
+1. Build the Docker Container:
+
+    ```
+    docker build -t moveit_frankapy .
+    ```
+
+2. Run Docker Container:
+    ```
+    cd moveit_frankapy
+    bash run_docker.sh
+    ```
+
+    Run A Terminal connected to the Docker Container:
+    ```
+    cd moveit_frankapy
+    bash terminal_docker.sh
+    ```
+
+3. Run the MoveIt launch file <br>
+    In a terminal connected to the Docker Container:
+    ```
+    roslaunch manipulation demo_frankapy.launch
+    ```
+
+4. Run the demo_moveit.py script <br>
+    In a terminal connected to the Docker Container:
+    ```
+    rosrun manipulation demo_moveit.py
+    ```
+

moveit_frankapy/run_docker.sh

@@ -0,0 +1,14 @@
+xhost +local:root 
+docker container prune -f 
+docker run --privileged --rm -it \
+    --name="moveit_frankapy" \
+    --env="DISPLAY=$DISPLAY" \
+    --env="QT_X11_NO_MITSHM=1" \
+    --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
+    --volume="$XAUTH:$XAUTH" \
+    --network host \
+    -v $(pwd)/src/devel_packages:/home/ros_ws/src/devel_packages \
+    moveit_frankapy \
+    bash
+
+# NOTE: --network host is used to allow the container to access the host's network

moveit_frankapy/src/devel_packages/manipulation/CMakeLists.txt

@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(manipulation)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  std_msgs
+)
+
+## Declare a catkin package
+catkin_package()
+
+## Specify locations of header files
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Add executables
+catkin_install_python(PROGRAMS
+  src/demo_moveit.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+catkin_install_python(PROGRAMS
+  src/robot_joint_converter.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark other files for installation (e.g. launch and config files)
+install(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

moveit_frankapy/src/devel_packages/manipulation/launch/demo_frankapy.launch

@@ -0,0 +1,70 @@
+<!-- A modified version of the panda_moveit_config/launch/demo.launch file -->
+<launch>
+  <arg name="arm_id" default="panda" />
+
+  <!-- specify the planning pipeline -->
+  <arg name="pipeline" default="ompl" />
+
+  <!-- By default, we do not start a database (it can be large) -->
+  <arg name="db" default="false" />
+  <!-- Allow user to specify database location -->
+  <arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db" />
+
+  <!-- By default, we are not in debug mode -->
+  <arg name="debug" default="false" />
+
+  <!-- By default we will load the gripper -->
+  <arg name="load_gripper" default="true" />
+
+  <!-- By default, we will load or override the robot_description -->
+  <arg name="load_robot_description" default="true"/>
+
+  <!-- Choose controller manager: fake, simple, or ros_control -->
+  <arg name="moveit_controller_manager" default="fake" />
+  <!-- Set execution mode for fake execution controllers -->
+  <arg name="fake_execution_type" default="interpolate" />
+  <!-- Transmission used for joint control: position, velocity, or effort -->
+  <arg name="transmission" />
+
+  <!-- By default, hide joint_state_publisher's GUI in 'fake' controller_manager mode -->
+  <arg name="use_gui" default="false" />
+  <arg name="use_rviz" default="true" />
+  <!-- Use rviz config for MoveIt tutorial -->
+  <arg name="rviz_tutorial" default="false" />
+
+  <!-- If needed, broadcast static tf for robot root -->
+  <!-- <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world $(arg arm_id)_link0" /> -->
+
+  <group if="$(eval arg('moveit_controller_manager') == 'fake')">
+    <!-- Real robot joints are remapped on the /real_robot_joints topic, which the joint_state_publisher uses -->
+    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
+      <rosparam param="source_list">[real_robot_joints]</rosparam> 
+      <!-- <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam> -->
+    </node>
+
+    <!-- Given the published joint states, publish tf for the robot links -->
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+  </group>
+
+  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
+  <include file="$(find panda_moveit_config)/launch/move_group.launch" pass_all_args="true">
+    <arg name="allow_trajectory_execution" value="true" />
+    <arg name="info" value="true" />
+  </include>
+
+  <!-- Run Rviz and load the default config to see the state of the move_group node -->
+  <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" if="$(arg use_rviz)">
+    <arg name="rviz_tutorial" value="$(arg rviz_tutorial)"/>
+    <arg name="rviz_config" value="$(find panda_moveit_config)/launch/moveit.rviz"/>
+    <arg name="debug" value="$(arg debug)"/>
+  </include>
+
+  <!-- If database loading was enabled, start mongodb as well -->
+  <include file="$(find panda_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
+    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
+  </include>
+
+  <!-- Launch robot_joint_converter node -->
+  <node name="robot_joint_converter" pkg="manipulation" type="robot_joint_converter.py" output="screen" />
+
+</launch>

moveit_frankapy/src/devel_packages/manipulation/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>manipulation</name>
+  <version>0.0.1</version>
+  <description>
+    A package which allows the use of moveit with frankapy.
+  </description>
+
+  <maintainer email="[email protected]">Vibhakar Mohta</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+  <export>
+    <build_type>catkin</build_type>
+  </export>
+</package>