franka_ros: Panda with mobile robot gazebo crash

Hi Everyone,

because I couldn’t find any solution, I decided to write about my issue.

On ROS Noetic I try to put Panda robot on the Clearpath dingo robot. I did it with xacro file

<?xml version="1.0"?>
<robot name="mobile_platform" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:include filename="$(find dingo_description)/urdf/dingo.urdf.xacro" ns="platform"/>
  <xacro:include filename="$(find franka_description)/robots/panda/panda.urdf.xacro" ns="arm"/>
  
  <xacro:include filename="$(find mobile-platform)/urdf/_d435.urdf.xacro"/>
  <xacro:sensor_d435 parent="base_link" topics_ns="camera_front">
    <origin xyz="0.35  0 0.1" rpy="0 -0.13 0"/>
  </xacro:sensor_d435>

  <joint name="panda_base_joint" type="fixed">
    <parent link="base_link"/>
    <child link="panda_link0"/>
    <origin xyz="0 0 0.1"/>
  </joint>
</robot>

After running roslaunch urdf_tutorial display.launch model:=mobile-platform-with-arm.urdf.xacro in rviz everything looks fine, but when I try to launch everything in gazebo with this launch

<launch>
    <!-- node for platform -->
    <arg name="config" default="$(optenv DINGO_CONFIG front_laser)" />
    <arg name="use_sim_time" default="true" />
    <arg name="gui" default="true" />
    <arg name="headless" default="false" />
    <arg name="world_name" default="office_small.world" />
    <arg name="arm_id" default="panda"/>
    <arg name="arm_namespace" default="panda"/>
    <!-- <arg name="x"         default="0"/>
    <arg name="y"         default="0"/>
    <arg name="z"         default="0.1"/>
    <arg name="yaw"       default="0"/> -->
    
    <!-- <rosparam command="load" file="$(find dingo_gazebo)/config/control_omni.yaml" /> -->
    
    <param name="robot_description"
      command="$(find dingo_description)/scripts/env_run
      $(find dingo_description)/urdf/configs/$(arg config)
      $(find xacro)/xacro $(find mobile-platform)/urdf/mobile-platform-with-arm.urdf.xacro" />
  
    <rosparam file="$(find mobile-platform)/config/gains.yaml" subst_value="true"/>
    <rosparam file="$(find franka_gazebo)/config/sim_controllers.yaml" subst_value="true" ns="arm"/>
    <rosparam command="load" file="$(find dingo_gazebo)/config/gains_omni.yaml" ns="platform"/>
  
    <param name="m_ee" value="0.76" />  

    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" /> 
    <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher">
      <rosparam param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
      <param name="rate" value="30"/>
    </node>  

   <!-- <remap from="/dingo_velocity_controller/cmd_vel" to="/cmd_vel"/> -->

    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"
          args="-urdf -model mobile_platform -param robot_description" />
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="debug" value="0" />
        <arg name="gui" value="$(arg gui)" />
        <arg name="use_sim_time" value="$(arg use_sim_time)" />
        <arg name="headless" value="$(arg headless)" />
        <arg name="world_name" value="$(arg world_name)" />
        <arg name="paused" value="true"/>
    </include>
  </launch>

Gazebo is launching, robot is spawned and then gzserver crashes. image image

franka_hw_sim checks joints to register, but because of the dingo robot, the amount of joints is bigger than 7. I tried to change namespaces for ros_control plugins in dingo and panda urdf’s but I am clueless how to solve it.

I hope I asked in proper place. Separately, both robots are working, but launching them in one simulation to create mobile panda exceed my skills.

Command line after launching with errors and warnings I put in file below. command.txt

It would be awesome if someone could give me a hint to solve this.

About this issue

  • Original URL
  • State: closed
  • Created a year ago
  • Comments: 15 (8 by maintainers)

Commits related to this issue

Most upvoted comments

Hey @kosmonauta144,

I finally got some time to investigate your issue.

It seems your problem is actually a bug inside FrankaHWSim. Here we blindly assumed, that all joints with a transmission interface belong to the robot. This obviously breaks apart in your case, where you have URDF with joints from different “subrobots” so to say.

I created a PR which could solve your problem, can you please try to pull that branch and see if it works for you?

I cheated a little bit with this gazebo:=true parameter and I locally removed the part with adding world link

Aha I see 😉 Note that in the same branch I added a parent (and base_xyz and base_rpy) xacro argument, so you don’t have to manually adjust the URDF anymore. Now something like this should work for you:

<robot name="mobile_platform_with_arm">
  <xacro:include filename="$(find dingo_description)/urdf/dingo.urdf.xacro" ns="platform"/>
  <xacro:include filename="$(find franka_description)/robots/common/franka_robot.xacro" />
  
  <!-- instantiate your platform here. Assuming it has a link called `base_link` -->
  
  <!-- Attach `fr3_link0` to `base_link`, 25cm above it -->
   <xacro:franka_robot
         robot="fr3"
         parent="base_link"
         base_xyz="0 0 0.25">
  </xacro:franka_robot>
</robot>

Would be great if you can also test that out and give us feedback.

Hope this helps