Humble hande integration (#18)

docker/ur-driver/Dockerfile

@@ -16,7 +16,25 @@ RUN apt-get update \
 RUN pip3 install rosdep && \
         rosdep update
 
+RUN mkdir /root/ws/src -p
+
+WORKDIR /root/ws/src
+
+RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_integration
+
+WORKDIR /root/ws/
+
+RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \
+        colcon build --symlink-install --packages-select ur3e_hande_robot_description"
+
 ENV ROBOT_IP=192.168.56.101
 ENV REVERSE_IP=192.168.56.102
 ENV UR_TYPE="ur3e"
-CMD ["/bin/sh", "-c", ".  /opt/ros/${ROS_DISTRO}/setup.sh &&  ros2 launch ur_robot_driver ur_control.launch.py ur_type:=${UR_TYPE} robot_ip:=${ROBOT_IP} launch_rviz:=false"]
+ENV DESCRIPTION_PKG="ur3e_hande_robot_description"
+ENV DESCRIPTION_FILE="ur_with_hande.urdf.xacro"
+
+# Copy entrypoint script
+COPY entrypoint.sh /entrypoint.sh
+RUN chmod +x /entrypoint.sh
+
+ENTRYPOINT [ "/entrypoint.sh" ]

docker/ur-driver/entrypoint.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+
+echo "Connecting to simulator through ursim for ur_type ${UR_TYPE}..."
+.  /opt/ros/${ROS_DISTRO}/setup.bash
+.  /root/ws/install/setup.bash 
+
+ros2 launch ur_robot_driver ur_control.launch.py ur_type:=${UR_TYPE} robot_ip:=${ROBOT_IP} launch_rviz:=false description_package:=${DESCRIPTION_PKG} description_file:=${DESCRIPTION_FILE}

docker/ur-moveit/Dockerfile

@@ -18,10 +18,28 @@ RUN apt-get autoremove -y \
 RUN pip3 install rosdep && \
         rosdep update
 
+RUN mkdir /root/ws/src -p
+
+WORKDIR /root/ws/src
+
+RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_integration
+
+WORKDIR /root/ws/
+
+RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \
+        colcon build --symlink-install --packages-select ur3e_hande_robot_description && \
+        colcon build --symlink-install --packages-select ur3e_hande_moveit_config"
+
+WORKDIR /root/ws/
+
 ENV ROBOT_IP=192.168.56.101
 ENV REVERSE_IP=192.168.56.102
 ENV UR_TYPE="ur3e"
 ENV LAUNCH_RVIZ="false"
+ENV DESCRIPTION_PKG="ur3e_hande_robot_description"
+ENV DESCRIPTION_FILE="ur_with_hande.urdf.xacro"
+ENV CONFIG_PKG="ur3e_hande_moveit_config"
+ENV CONFIG_FILE="ur.srdf"
 
 # Copy entrypoint script
 COPY entrypoint.sh /entrypoint.sh

docker/ur-moveit/entrypoint.sh

@@ -8,4 +8,6 @@ x11vnc -rfbport 5901 -bg -quiet -forever -shared -display :1
 
 echo "Starting rviz and moveit with ur_type ${UR_TYPE}..."
 .  /opt/ros/${ROS_DISTRO}/setup.bash
-ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=${UR_TYPE} launch_rviz:=${LAUNCH_RVIZ}
+. /root/ws/install/setup.bash
+
+ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=${UR_TYPE} launch_rviz:=${LAUNCH_RVIZ} description_package:=${DESCRIPTION_PKG}  launch_servo:=false description_file:=${DESCRIPTION_FILE} moveit_config_package:=${CONFIG_PKG} moveit_config_file:=${CONFIG_FILE}

src/pdf_beamtime/config/joint_poses.yaml

@@ -1,3 +1,4 @@
 pdf_beamtime_server:
   ros__parameters:
     home_angles: [-0.11536, -1.783732, 0.38816, -1.75492, 0.11484, 3.14159] #For real robot
+    gripper_present: false

src/pdf_beamtime/include/pdf_beamtime/pdf_beamtime_server.hpp

@@ -71,6 +71,8 @@ class PdfBeamtimeServer
   const float total_states_ = 9.0;
   float progress_ = 0.0;
 
+  bool gripper_present_ = false;
+
   // Action server related callbacks
   rclcpp_action::GoalResponse handle_goal(
     const rclcpp_action::GoalUUID & uuid,

src/pdf_beamtime/src/pdf_beamtime_server.cpp

@@ -49,6 +49,7 @@ PdfBeamtimeServer::PdfBeamtimeServer(
 
   // // Initialize to home
   current_state_ = State::HOME;
+  gripper_present_ = node_->get_parameter("gripper_present").as_bool();
 }
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr PdfBeamtimeServer::getNodeBaseInterface()
 // Expose the node base interface so that the node can be added to a component manager.
@@ -329,8 +330,9 @@ bool PdfBeamtimeServer::run_fsm(
       break;
 
     case State::PICKUP:
-      // TODO(chandimafernando): Add a check for gipper availability before open/close
-      // gripper_close();
+      if (this->gripper_present_) {
+        // gripper_close()
+      }
       state_transition = true;
       break;
 
@@ -347,7 +349,9 @@ bool PdfBeamtimeServer::run_fsm(
       break;
 
     case State::PLACE:
-      // gripper_open();
+      if (this->gripper_present_) {
+        // gripper_open();
+      }
       state_transition = true;
       break;
 
@@ -382,7 +386,9 @@ bool PdfBeamtimeServer::reset_fsm(std::vector<double> joint_goal)
 {
   RCLCPP_INFO(node_->get_logger(), "State machine was RESET");
   current_state_ = State::HOME;
-  // gripper_open();
+  if (this->gripper_present_) {
+    // gripper_open();
+  }
   return set_joint_goal(joint_goal);
 }
 

src/ur3e_hande_moveit_config/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.8)
+project(ur3e_hande_moveit_config)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+
+install(DIRECTORY config rviz srdf
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/ur3e_hande_moveit_config/README.md

@@ -0,0 +1,17 @@
+## ur3e_hande_moveit_config
+
+This package defines configuration parameters of the combined robotic system of hand-e gripper and ur3e robot arm. 
+Included files are autogenerated through the "MoveIt Setup Assistant" program and modified according to the need afterwards. 
+
+/srdf/ur.srdf is the key file that was generated through the "MoveIt Setup Assistant" program. It defines the collision matrix for the custom robot+arm configuration.
+The input the "MoveIt Setup Assistant" program is a .urdf file that combines both the design files for the ur3e robot arm and the hande gripper. 
+Generating the .urdf file is described below:
+
+In this case, the combined .xacro file is named as ur.urdf.xacro. **In our current setup, this file is /ur3e_hande_robot_description/urdf/ur_with_hande.xacro.**
+
+We use the ur.urdf.xacro file to generate the ur.urdf file using the following command, **(make sure to correct the package names, and file names to the appropriate names/locations of your workspace.)**
+```bash
+ros2 run xacro xacro $(ros2 pkg prefix ur3e_hande_description)/share/ur3e_hande_description/urdf/ur.urdf.xacro name:=ur ur_type:=ur3e joint_limit_params:=$(ros2 pkg prefix ur3e_hande_description)/share/ur3e_hande_description/config/ur3e/joint_limits.yaml kinematics_params:=$(ros2 pkg prefix ur3e_hande_description)/share/ur3e_hande_description/config/ur3e/default_kinematics.yaml physical_params:=$(ros2 pkg prefix ur3e_hande_description)/share/ur3e_hande_description/config/ur3e/physical_parameters.yaml visual_params:=$(ros2 pkg prefix ur3e_hande_description)/share/ur3e_hande_description/config/ur3e/visual_parameters.yaml > ur.urdf
+```
+
+Next, follow [this useful link on using MoveIt Setup Assistant](https://gramaziokohler.github.io/compas_fab/0.16.0/examples/03_backends_ros/08_ros_create_moveit_package_from_custom_urdf.html) to generate the ur.srdf file. This process will end up generating other useful files such as /rviz/view_robot.rviz

src/ur3e_hande_moveit_config/config/kinematics.yaml

@@ -0,0 +1,8 @@
+/**:
+  ros__parameters:
+    robot_description_kinematics:
+      ur_arm:
+        kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+        kinematics_solver_search_resolution: 0.005
+        kinematics_solver_timeout: 0.005
+        kinematics_solver_attempts: 3

src/ur3e_hande_moveit_config/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ur3e_hande_moveit_config</name>
+  <version>0.0.0</version>
+  <description>Configuration parameters of the combined robotic system of hand-e gripper and ur3e robot arm</description>
+  <maintainer email="wfernando1@bnl.gov">wfernando1</maintainer>
+  <license>BSD-3</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/ur3e_hande_moveit_config/rviz/view_robot.rviz

@@ -0,0 +1,51 @@
+Panels:
+  - Class: rviz_common/Displays
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /MotionPlanning1
+  - Class: rviz_common/Help
+    Name: Help
+  - Class: rviz_common/Views
+    Name: Views
+Visualization Manager:
+  Displays:
+    - Class: rviz_default_plugins/Grid
+      Name: Grid
+      Value: true
+    - Class: moveit_rviz_plugin/MotionPlanning
+      Name: MotionPlanning
+      Planned Path:
+        Loop Animation: true
+        State Display Time: 0.05 s
+        Trajectory Topic: display_planned_path
+      Planning Scene Topic: monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 1
+      Scene Robot:
+        Robot Alpha: 0.5
+      Value: true
+  Global Options:
+    Fixed Frame: world
+  Tools:
+    - Class: rviz_default_plugins/Interact
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 2.0
+      Focal Point:
+        X: -0.1
+        Y: 0.25
+        Z: 0.30
+      Name: Current View
+      Pitch: 0.5
+      Target Frame: world
+      Yaw: -0.623
+Window Geometry:
+  Height: 975
+  QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Width: 1200

src/ur3e_hande_moveit_config/srdf/ur.srdf

@@ -0,0 +1,67 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot name="ur">
+    <!--GROUPS Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+    <!--LINKS When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+    <!--JOINTS When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+    <!--CHAINS When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+    <!--SUBGROUPS Groups can also be formed by referencing to already defined group names-->
+    <group name="ur_arm">
+        <joint name="virtual_joint"/>
+        <joint name="base_joint"/>
+        <joint name="base_link-base_link_inertia"/>
+        <joint name="shoulder_pan_joint"/>
+        <joint name="shoulder_lift_joint"/>
+        <joint name="elbow_joint"/>
+        <joint name="wrist_1_joint"/>
+        <joint name="wrist_2_joint"/>
+        <joint name="wrist_3_joint"/>
+        <joint name="wrist_3-flange"/>
+    </group>
+    <group name="hand">
+        <link name="robotiq_hande_base_link"/>
+        <link name="left_finger"/>
+        <link name="right_finger"/>
+    </group>
+    <!--GROUP STATES Purpose Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
+    <group_state name="moveit_home" group="ur_arm">
+        <joint name="elbow_joint" value="0.8505"/>
+        <joint name="shoulder_lift_joint" value="-0.8678"/>
+        <joint name="shoulder_pan_joint" value="0"/>
+        <joint name="wrist_1_joint" value="-0.1041"/>
+        <joint name="wrist_2_joint" value="1.5621"/>
+        <joint name="wrist_3_joint" value="0"/>
+    </group_state>
+    <!--END EFFECTOR Purpose Represent information about an end effector.-->
+    <end_effector name="hand" parent_link="flange" group="hand"/>
+    <!--VIRTUAL JOINT Purpose this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
+    <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="right_finger"/>
+    <!--DISABLE COLLISIONS By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+    <disable_collisions link1="base_link_inertia" link2="shoulder_link" reason="Adjacent"/>
+    <disable_collisions link1="base_link_inertia" link2="upper_arm_link" reason="Never"/>
+    <disable_collisions link1="base_link_inertia" link2="wrist_1_link" reason="Never"/>
+    <disable_collisions link1="base_link_inertia" link2="wrist_2_link" reason="Never"/>
+    <disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent"/>
+    <disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent"/>
+    <disable_collisions link1="left_finger" link2="right_finger" reason="Never"/>
+    <disable_collisions link1="left_finger" link2="robotiq_hande_base_link" reason="Adjacent"/>
+    <disable_collisions link1="left_finger" link2="wrist_1_link" reason="Never"/>
+    <disable_collisions link1="left_finger" link2="wrist_2_link" reason="Never"/>
+    <disable_collisions link1="left_finger" link2="wrist_3_link" reason="Never"/>
+    <disable_collisions link1="right_finger" link2="robotiq_hande_base_link" reason="Adjacent"/>
+    <disable_collisions link1="right_finger" link2="wrist_1_link" reason="Never"/>
+    <disable_collisions link1="right_finger" link2="wrist_2_link" reason="Never"/>
+    <disable_collisions link1="right_finger" link2="wrist_3_link" reason="Never"/>
+    <disable_collisions link1="robotiq_hande_base_link" link2="wrist_1_link" reason="Never"/>
+    <disable_collisions link1="robotiq_hande_base_link" link2="wrist_2_link" reason="Never"/>
+    <disable_collisions link1="robotiq_hande_base_link" link2="wrist_3_link" reason="Adjacent"/>
+    <disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent"/>
+    <disable_collisions link1="shoulder_link" link2="wrist_1_link" reason="Never"/>
+    <disable_collisions link1="shoulder_link" link2="wrist_2_link" reason="Never"/>
+    <disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent"/>
+    <disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never"/>
+    <disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent"/>
+</robot>

src/ur3e_hande_robot_description/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.8)
+project(ur3e_hande_robot_description)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+
+install(DIRECTORY config meshes urdf
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/ur3e_hande_robot_description/README.md

@@ -0,0 +1,53 @@
+## ur3e_hande_robot_description
+
+This package contains 
+  - descriptions files for the hand-e robotic gripper by *Robotiq*,
+  - description file for integrating the gripper to ur3e robotic arm by Universal Robotics, and
+  - four configuration files to be passed when initiating the ur3e robotic arm. 
+
+
+**config/hande**
+
+Files default_kinematics.yaml, physical_parameters.yaml, and visual_parameters are created both as a part of the automated process through "MoveIt Setup Assistant" and extracting the values from hand-e user manual. 
+These files contain the physical properties of the hand-e gripper and pointers to the respective design files
+
+**config/ur3e**
+
+Configuration files for the ur3e robot are directly copied from the humble version of the package [*Universal_Robots_ROS2_Description*](https://github.com/UniversalRobots/Universal_Robots_ROS2_Description/tree/29e90d5095fdf4af99eba3c3eae153d7d5d769c0/config/ur3e).
+These configurations are free to be adjusted. 
+
+**meshes/hande**
+
+These files contains designs for the hand-e robot. .dae files are directly taken from *Acutronic Robotic*'s [robotiq_modular_gripper](https://github.com/AcutronicRobotics/robotiq_modular_gripper/tree/4e708524e5dd20753f711686eb2cd1017a25a09e/robotiq_hande_gripper_description/meshes). These design files were not available in *Robotiq*'s github repositories.
+
+**urdf/hande.xacro**
+
+The description file for the hand-e gripper. This is inspired from *Acutronic Robotic*'s [robotiq_modular_gripper](https://github.com/AcutronicRobotics/robotiq_modular_gripper/blob/4e708524e5dd20753f711686eb2cd1017a25a09e/robotiq_hande_gripper_description/urdf/robotiq_hande.urdf.xacro) and extended to include paramters from config/hande. 
+
+**urdf/ur_with_hande.urdf.xacro**
+
+This file creates the description for the ur3e robot and attaches the hande gripper. Ur3e robot module is created to be directly compatible with the humble version of the the package [ur_description](https://github.com/UniversalRobots/Universal_Robots_ROS2_Description/blob/29e90d5095fdf4af99eba3c3eae153d7d5d769c0/urdf/ur.urdf.xacro) by the following line 
+```xml
+<xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>
+```
+.
+
+## Execution
+
+Start up the ur simulator program (IP to be connected via a VNC viewer: 192.168.56.101):
+```bash
+docker compose up ursim
+```
+
+Build and start the ur_driver program:
+```bash
+docker compose up urdriver 
+```
+
+Build the ur3e_hande_robot_description and ur3e_hande_moveit_config packages by running (IP to be connected via a VNC viewer: 192.168.56.103:5901):
+```bash
+docker build -t ur-moveit:latest .
+docker compose up urmoveit
+```
+
+