Merge pull request #1 from LCAS/humble

Vision integration
yilmazabdurrah · Sep 20, 2024 · f330136 · f330136
2 parents 6494514 + e14a3b9
commit f330136
Show file tree

Hide file tree

Showing 14 changed files with 404 additions and 64 deletions.
diff --git a/.github/workflows/ros-ci.yml b/.github/workflows/ros-ci.yml
@@ -0,0 +1,68 @@
+name: ros CI
+
+on:
+  push:
+    # you may want to configure the branches that this should be run on here.
+    branches: [ "humble" ]
+  pull_request:
+    branches: [ "humble" ]
+
+jobs:
+  test_docker: # On Linux, iterates on all ROS 1 and ROS 2 distributions.
+    runs-on: ubuntu-latest
+    strategy:
+      matrix:
+        ros_distribution:
+          # - noetic
+          - humble
+          # - iron
+
+        # Define the Docker image(s) associated with each ROS distribution.
+        # The include syntax allows additional variables to be defined, like
+        # docker_image in this case. See documentation:
+        # https://help.github.com/en/actions/reference/workflow-syntax-for-github-actions#example-including-configurations-in-a-matrix-build
+        #
+        # Platforms are defined in REP 3 and REP 2000:
+        # https://ros.org/reps/rep-0003.html
+        # https://ros.org/reps/rep-2000.html
+        include:
+          # Noetic Ninjemys (May 2020 - May 2025)
+          # - docker_image: ubuntu:focal
+          #   ros_distribution: noetic
+          #   ros_version: 1
+
+          # Humble Hawksbill (May 2022 - May 2027)
+          - docker_image: ubuntu:jammy
+            ros_distribution: humble
+            ros_version: 2
+
+          # Iron Irwini (May 2023 - November 2024)
+          # - docker_image: ubuntu:jammy
+          #   ros_distribution: iron
+          #   ros_version: 2
+
+          # # Rolling Ridley (No End-Of-Life)
+          # - docker_image: ubuntu:jammy
+          #   ros_distribution: rolling
+          #   ros_version: 2
+
+    container:
+      image: ${{ matrix.docker_image }}
+    steps:
+      - uses: actions/checkout@v3
+      - name: setup ROS environment
+        uses: LCAS/setup-ros@master
+        with:
+          required-ros-distributions: ${{ matrix.ros_distribution }}
+      - name: build and test ROS 1
+        if: ${{ matrix.ros_version == 1 }}
+        uses: ros-tooling/action-ros-ci@v0.3
+        with:
+          import-token: ${{ github.token }}
+          target-ros1-distro: ${{ matrix.ros_distribution }}
+      - name: build and test ROS 2
+        if: ${{ matrix.ros_version == 2 }}
+        uses: ros-tooling/action-ros-ci@v0.3
+        with:
+          import-token: ${{ github.token }}
+          target-ros2-distro: ${{ matrix.ros_distribution }}
diff --git a/franka_bringup/config/controllers.yaml b/franka_bringup/config/controllers.yaml
@@ -44,7 +44,7 @@ joint_trajectory_controller:
       - panda_joint6
       - panda_joint7
     command_interfaces:
-      - effort
+      - velocity
     state_interfaces:
       - position
       - velocity
@@ -153,4 +153,4 @@ cartesian_velocity_example_controller:
       - 1.0
       - 1.0
       - 1.0
-      - 0.5
+      - 0.5
diff --git a/franka_description/launch/visualize_franka.launch.py b/franka_description/launch/visualize_franka.launch.py
@@ -26,7 +26,7 @@ def generate_launch_description():
     load_gripper = LaunchConfiguration(load_gripper_parameter_name)
 
     franka_xacro_file = os.path.join(get_package_share_directory('franka_description'), 'robots',
-                                     'panda_arm.urdf.xacro')
+                                     'panda_arm_camera.urdf.xacro')
     robot_description = Command(
         [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=', load_gripper])
 

diff --git a/franka_description/robots/panda_arm.ros2_control.xacro b/franka_description/robots/panda_arm.ros2_control.xacro
@@ -27,14 +27,14 @@
         </joint>
       </xacro:macro>
 
-      <xacro:configure_joint joint_name="${ns}_joint1" initial_position="0.0"/>
-      <xacro:configure_joint joint_name="${ns}_joint2" initial_position="${-pi/4}"/>
+      <xacro:configure_joint joint_name="${ns}_joint1" initial_position="-1.57"/>
+      <xacro:configure_joint joint_name="${ns}_joint2" initial_position="0.67"/>
       <xacro:configure_joint joint_name="${ns}_joint3" initial_position="0.0"/>
-      <xacro:configure_joint joint_name="${ns}_joint4" initial_position="${-3*pi/4}"/>
+      <xacro:configure_joint joint_name="${ns}_joint4" initial_position="0.0"/>
       <xacro:configure_joint joint_name="${ns}_joint5" initial_position="0.0"/>
-      <xacro:configure_joint joint_name="${ns}_joint6" initial_position="${pi/2}"/>
-      <xacro:configure_joint joint_name="${ns}_joint7" initial_position="${pi/4}"/>
+      <xacro:configure_joint joint_name="${ns}_joint6" initial_position="1.3"/>
+      <xacro:configure_joint joint_name="${ns}_joint7" initial_position="2.3"/>
 
     </ros2_control>
   </xacro:macro>
-</robot>
+</robot>
diff --git a/franka_description/robots/panda_arm.xacro b/franka_description/robots/panda_arm.xacro
@@ -61,12 +61,12 @@
       </collision>
     </link>
     <joint name="${arm_id}_joint1" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.7473" soft_upper_limit="2.7473"/>
       <origin rpy="0 0 0" xyz="0 0 0.333"/>
       <parent link="${arm_id}_link0"/>
       <child link="${arm_id}_link1"/>
       <axis xyz="0 0 1"/>
-      <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+      <limit effort="87" lower="-2.7473" upper="2.7473" velocity="2.1750"/>
     </joint>
     <link name="${arm_id}_link2">
       <visual>
@@ -94,12 +94,12 @@
       </collision>
     </link>
     <joint name="${arm_id}_joint2" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.6728" soft_upper_limit="1.6728"/>
       <origin rpy="${-pi/2} 0 0" xyz="0 0 0"/>
       <parent link="${arm_id}_link1"/>
       <child link="${arm_id}_link2"/>
       <axis xyz="0 0 1"/>
-      <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
+      <limit effort="87" lower="-1.6728" upper="1.6728" velocity="2.1750"/>
     </joint>
     <link name="${arm_id}_link3">
       <visual>
@@ -127,12 +127,12 @@
       </collision>
     </link>
     <joint name="${arm_id}_joint3" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.7473" soft_upper_limit="2.7473"/>
       <origin rpy="${pi/2} 0 0" xyz="0 -0.316 0"/>
       <parent link="${arm_id}_link2"/>
       <child link="${arm_id}_link3"/>
       <axis xyz="0 0 1"/>
-      <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+      <limit effort="87" lower="-2.7473" upper="2.7473" velocity="2.1750"/>
     </joint>
     <link name="${arm_id}_link4">
       <visual>
@@ -160,12 +160,12 @@
       </collision>
     </link>
     <joint name="${arm_id}_joint4" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.9218" soft_upper_limit="-0.2198"/>
       <origin rpy="${pi/2} 0 0" xyz="0.0825 0 0"/>
       <parent link="${arm_id}_link3"/>
       <child link="${arm_id}_link4"/>
       <axis xyz="0 0 1"/>
-      <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
+      <limit effort="87" lower="-2.9218" upper="-0.2198" velocity="2.1750"/>
     </joint>
     <link name="${arm_id}_link5">
       <visual>
@@ -212,12 +212,12 @@
 
     </link>
     <joint name="${arm_id}_joint5" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.7473" soft_upper_limit="2.7473"/>
       <origin rpy="${-pi/2} 0 0" xyz="-0.0825 0.384 0"/>
       <parent link="${arm_id}_link4"/>
       <child link="${arm_id}_link5"/>
       <axis xyz="0 0 1"/>
-      <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+      <limit effort="12" lower="-2.7473" upper="2.7473" velocity="2.6100"/>
     </joint>
     <link name="${arm_id}_link6">
       <visual>
@@ -245,12 +245,12 @@
       </collision>
     </link>
     <joint name="${arm_id}_joint6" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0775" soft_upper_limit="3.6025"/>
       <origin rpy="${pi/2} 0 0" xyz="0 0 0"/>
       <parent link="${arm_id}_link5"/>
       <child link="${arm_id}_link6"/>
       <axis xyz="0 0 1"/>
-      <limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
+      <limit effort="12" lower="-0.0775" upper="3.6025" velocity="2.6100"/>
     </joint>
     <link name="${arm_id}_link7">
       <visual>
@@ -278,12 +278,12 @@
       </collision>
     </link>
     <joint name="${arm_id}_joint7" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.7473" soft_upper_limit="2.7473"/>
       <origin rpy="${pi/2} 0 0" xyz="0.088 0 0"/>
       <parent link="${arm_id}_link6"/>
       <child link="${arm_id}_link7"/>
       <axis xyz="0 0 1"/>
-      <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+      <limit effort="12" lower="-2.7473" upper="2.7473" velocity="2.6100"/>
     </joint>
     <link name="${arm_id}_link8">
       <collision>

diff --git a/franka_description/robots/panda_arm_platform.urdf.xacro b/franka_description/robots/panda_arm_platform.urdf.xacro
@@ -0,0 +1,24 @@
+<?xml version='1.0' encoding='utf-8'?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+  <xacro:arg name="arm_id" default="panda"/> <!-- Name of this panda -->
+  <xacro:arg name="hand" default="false"/> <!-- Should a franka_gripper be mounted at the flange? -->
+  <xacro:arg name="camera" default="true"/> <!-- Should a camera be mounted at the flange? -->
+  <xacro:arg name="camera_model" default="blackfly_s"/> <!-- Which camera model will be used -->
+  <xacro:arg name="robot_ip" default=""/> <!-- IP address or hostname of the robot" -->
+  <xacro:arg name="use_fake_hardware" default="false"/>
+  <xacro:arg name="fake_sensor_commands" default="false"/>
+
+  <xacro:include filename="$(find franka_description)/robots/panda_arm.xacro"/>
+  <xacro:panda_arm arm_id="$(arg arm_id)" safety_distance="0.03"/>
+
+  <xacro:if value="$(arg hand)">
+    <xacro:include filename="$(find franka_description)/robots/hand.xacro"/>
+    <xacro:hand ns="$(arg arm_id)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id)_link8" safety_distance="0.03"/>
+  </xacro:if>
+  <xacro:if value="$(arg camera)">
+    <xacro:include filename="$(find flir_camera_description)/urdf/camera.urdf.xacro"/>
+    <xacro:camera ns="$(arg arm_id)" rpy="0 ${-pi/2} 0" xyz="0.01425 0 0.09" connected_to="$(arg arm_id)_link8"/>
+  </xacro:if>
+  <xacro:include filename="$(find franka_description)/robots/panda_arm.ros2_control.xacro"/>
+  <xacro:panda_arm_ros2_control ns="$(arg arm_id)" robot_ip="$(arg robot_ip)" use_fake_hardware="$(arg use_fake_hardware)" fake_sensor_commands="$(arg fake_sensor_commands)"/>
+</robot>
diff --git a/franka_moveit_config/config/kinematics.yaml b/franka_moveit_config/config/kinematics.yaml
@@ -10,3 +10,7 @@ right_arm:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
   kinematics_solver_timeout: 0.05
+panda_vision:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.05
diff --git a/franka_moveit_config/config/ompl_planning.yaml b/franka_moveit_config/config/ompl_planning.yaml
@@ -124,6 +124,8 @@ planner_configs:
     type: geometric::TrajOpt
 
 panda_arm:
+  enforce_constrained_state_space: true
+  projection_evaluator: joints(panda_joint1, panda_joint2)
   planner_configs:
     - SBLkConfigDefault
     - ESTkConfigDefault
@@ -150,6 +152,36 @@ panda_arm:
     - SPARStwokConfigDefault
     - TrajOptDefault
 panda_arm_hand:
+  enforce_constrained_state_space: true
+  projection_evaluator: joints(panda_joint1, panda_joint2)
+  planner_configs:
+    - SBLkConfigDefault
+    - ESTkConfigDefault
+    - LBKPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - KPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - RRTstarkConfigDefault
+    - TRRTkConfigDefault
+    - PRMkConfigDefault
+    - PRMstarkConfigDefault
+    - FMTkConfigDefault
+    - BFMTkConfigDefault
+    - PDSTkConfigDefault
+    - STRIDEkConfigDefault
+    - BiTRRTkConfigDefault
+    - LBTRRTkConfigDefault
+    - BiESTkConfigDefault
+    - ProjESTkConfigDefault
+    - LazyPRMkConfigDefault
+    - LazyPRMstarkConfigDefault
+    - SPARSkConfigDefault
+    - SPARStwokConfigDefault
+    - TrajOptDefault
+panda_vision:
+  enforce_constrained_state_space: true
+  projection_evaluator: joints(panda_joint1, panda_joint2)
   planner_configs:
     - SBLkConfigDefault
     - ESTkConfigDefault