Ignition Gazebo gripper ignores cube collision

edit

Hello,

I am using ros foxy with ignition citadel. I am testing ign_ros2_control using panda robot and when I am trying to grip a cube the hand simply goes through it. At the same time this happen there is a huge drop in the real time factor, which I assume is the physics engine detecting a collision and not knowing how to handle it. Is the error due to ros2_control or is it another problem ?

I am using this to spawn panda. The world file is based off this but the same behavior also happen if no world is provided.

<sdf version='1.7'>
<include>
  <uri>
  https://fuel.ignitionrobotics.org/1.0/AndrejOrsula/models/panda
  </uri>
  <plugin name='ignition_ros2_control::IgnitionROS2ControlPlugin' filename='ignition_ros2_control-system'>
    <robot_param>robot_description</robot_param>
    <robot_param_node>robot_state_publisher</robot_param_node>
    <parameters>/workspaces/Ignition/ubb/ignitiondev/install/share/simple_arm/config/panda_ros_controllers.yaml</parameters>
    <!-- Need to find a way to set this programatically -->
  </plugin>
</include>

</sdf> I am launching ignition using this launch file

import os

import xacro
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node

from launch import LaunchDescription
from launch.actions import (DeclareLaunchArgument, ExecuteProcess,
                            IncludeLaunchDescription)
from launch.launch_description_sources import PythonLaunchDescriptionSource


def load_file(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, 'r') as file:
            return file.read()
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
        return None


def generate_launch_description():
    pkg_name = "simple_arm"
    pkg_ros_ign_gazebo = get_package_share_directory('ros_ign_gazebo')
    pkg_share = get_package_share_directory(pkg_name)

    ignition = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py'),
        ),)

    spawn = Node(package='ros_ign_gazebo', executable='create',
                arguments=[
                    '-name', 'panda',
                    '-x', '0',
                    '-z', '0',
                    '-Y', '0',
                    '-file', os.path.join(pkg_share, "urdf", "panda_ign.sdf",)],
                output='screen')

    robot_description_config = xacro.process_file(
        os.path.join(
            pkg_share,
            "urdf",
            "panda.urdf.xacro",
        )
    )
    robot_description = {"robot_description": robot_description_config.toxml()}
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="both",
        parameters=[robot_description],
    )

    load_controllers = []
    for controller in [
        "panda_arm_controller",
        "panda_hand_controller",
        "joint_state_broadcaster",
    ]:
        load_controllers += [
            ExecuteProcess(
                cmd=[
                    "ros2 control load_controller --set-state start {}".format(controller)],
                shell=True,
                output="screen",
            )
        ]
    return LaunchDescription([
        DeclareLaunchArgument(
            'ign_args',
            default_value=[os.path.join(
                pkg_share, 'worlds', 'empty.sdf'), " -r"],
            description='Ignition Gazebo arguments'),
        ignition,
        spawn,
        robot_state_publisher,
    ] + load_controllers
    )

Moveit and rviz are launched using modified launch file from the move_group tutorial and the controller as well as advertizsing the object to rviz is custom made. I have not included them since i do not think they are relevant since all they do is tell moveit which object to move to.