search

frankaemika / franka_ros

ROS integration for Franka research robots
https://frankaemika.github.io
Apache License 2.0
364 stars 313 forks source link

Transmission has more than one joint launching dual panda in a single URDF #387

Open mikelasa opened 7 months ago

mikelasa commented 7 months ago

Hello, I'm working in a dual cartesian impedance control custom controller that I want to simulate in gazebo. This controller needs both franka_hw/FrankaStateInterface and franka_hw/FrankaModelInterface to compute some variables. I made a urdf.xacro file that holds both robots with different ids, but I'm having issues when add bot state and model code lines that I take from the original franka_robot.xacro file, which are: 

<!-- ========================================================== -->
  <!-- Adds the required tags to provide a `FrankaStateInterface` -->
  <!-- when simulating with franka_hw_sim plugin                  -->
  <!--                                                            -->
  <!-- arm_id - Arm ID of the panda to simulate (default 'panda') -->
  <!-- ========================================================== -->
  <xacro:macro name="transmission-franka-state" params="arm_id:=panda">
    <transmission name="${arm_id}_franka_state">
      <type>franka_hw/FrankaStateInterface</type>
      <joint name="${arm_id}_joint1"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
      <joint name="${arm_id}_joint2"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
      <joint name="${arm_id}_joint3"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
      <joint name="${arm_id}_joint4"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
      <joint name="${arm_id}_joint5"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
      <joint name="${arm_id}_joint6"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
      <joint name="${arm_id}_joint7"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>

      <actuator name="${arm_id}_motor1"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
      <actuator name="${arm_id}_motor2"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
      <actuator name="${arm_id}_motor3"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
      <actuator name="${arm_id}_motor4"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
      <actuator name="${arm_id}_motor5"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
      <actuator name="${arm_id}_motor6"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
      <actuator name="${arm_id}_motor7"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
    </transmission>
  </xacro:macro>

  <!-- ============================================================ -->
  <!-- Adds the required tags to provide a `FrankaModelInterface`   -->
  <!-- when simulating with franka_hw_sim plugin                    -->
  <!--                                                              -->
  <!-- arm_id - Arm ID of the panda to simulate (default 'panda')   -->
  <!-- root   - Joint name of the base of the robot                 -->
  <!-- tip    - Joint name of the tip of the robot (flange or hand) -->
  <!-- ============================================================ -->
  <xacro:macro name="transmission-franka-model" params="arm_id:=panda root:=panda_joint1 tip:=panda_joint7">
    <transmission name="${arm_id}_franka_model">
      <type>franka_hw/FrankaModelInterface</type>
      <joint name="${root}">
        <role>root</role>
        <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
      </joint>
      <joint name="${tip}">
        <role>tip</role>
        <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
      </joint>

      <actuator name="${root}_motor_root"><hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface></actuator>
      <actuator name="${tip}_motor_tip"  ><hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface></actuator>
    </transmission>
  </xacro:macro>

my dual_panda_2.urdf.xacro looks like this, I'm just taking code parts from franka_robot.xacro and adding them to this file:

<?xml version="1.0"?>
<robot name="dual_panda" xmlns:xacro="http://ros.org/wiki/xacro">

    <!-- add arms names prefixes -->
    <xacro:arg name="arm_id_1" default="panda_right" />
    <xacro:arg name="arm_id_2" default="panda_left" />

    <!-- load arm/hand models and utils (which adds the robot inertia tags to be Gazebo-simulation ready) -->
    <xacro:include filename="$(find franka_description)/robots/common/utils.xacro" />
    <xacro:include filename="$(find franka_description)/robots/common/franka_arm.xacro" />
    <xacro:include filename="$(find franka_description)/robots/common/franka_hand.xacro" />

    <link name="world"/>

    <!-- box shaped table as base for the 2 Pandas -->
    <link name="base">
        <visual>
            <origin xyz="0 0 0.5" rpy="0 0 0" />
            <geometry>
                <box size="1 2 1" />
            </geometry>
            <material name="White">
                <color rgba="1.0 1.0 1.0 1.0" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0.5" rpy="0 0 0" />
            <geometry>
                <box size="1 2 1" />
            </geometry>
        </collision>
        <inertial>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <mass value="10.0"/>
            <inertia ixx="0.001" ixy="0.0" ixz="0.001" iyy="0.0" iyz="0.0" izz="0.001"/>
        </inertial>

    </link>

    <joint name="base_to_world" type="fixed">
        <parent link="world"/>
        <child link="base"/>
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
    </joint>

    <!-- right arm with gripper -->
    <xacro:franka_arm arm_id="$(arg arm_id_1)" connected_to="base" xyz="0 -0.5 1" gazebo="true" safety_distance="0.03" joint_limits="${xacro.load_yaml('$(find franka_description)/robots/panda/joint_limits.yaml')}"/>
    <xacro:franka_hand arm_id="$(arg arm_id_1)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id_1)_link8" gazebo="true" safety_distance="0.03" />

    <!-- left arm with gripper -->
    <xacro:franka_arm arm_id="$(arg arm_id_2)" connected_to="base" xyz="0 0.5 1" gazebo="true" safety_distance="0.03" joint_limits="${xacro.load_yaml('$(find franka_description)/robots/panda/joint_limits.yaml')}"/>
    <xacro:franka_hand arm_id="$(arg arm_id_2)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id_2)_link8" gazebo="true" safety_distance="0.03" />

    <!-- right arm joints control interface -->
    <xacro:gazebo-joint joint="$(arg arm_id_1)_joint1" transmission="hardware_interface/EffortJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_1)_joint2" transmission="hardware_interface/EffortJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_1)_joint3" transmission="hardware_interface/EffortJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_1)_joint4" transmission="hardware_interface/EffortJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_1)_joint5" transmission="hardware_interface/EffortJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_1)_joint6" transmission="hardware_interface/EffortJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_1)_joint7" transmission="hardware_interface/EffortJointInterface" />

    <!-- left arm joints control interface -->
    <xacro:gazebo-joint joint="$(arg arm_id_2)_joint1" transmission="hardware_interface/EffortJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_2)_joint2" transmission="hardware_interface/EffortJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_2)_joint3" transmission="hardware_interface/EffortJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_2)_joint4" transmission="hardware_interface/EffortJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_2)_joint5" transmission="hardware_interface/EffortJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_2)_joint6" transmission="hardware_interface/EffortJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_2)_joint7" transmission="hardware_interface/EffortJointInterface" />

    <!-- transmission tags for the robot -->
    <xacro:transmission-franka-state arm_id="$(arg arm_id_1)" />
    <xacro:transmission-franka-model arm_id="$(arg arm_id_1)"
        root="$(arg arm_id_1)_joint1"
        tip="$(arg arm_id_1)_joint8"
    />

    <xacro:transmission-franka-state arm_id="$(arg arm_id_2)" />
    <xacro:transmission-franka-model arm_id="$(arg arm_id_2)"
        root="$(arg arm_id_2)_joint1"
        tip="$(arg arm_id_2)_joint8"
    />

    <!-- right hand joints control interface -->
    <xacro:gazebo-joint joint="$(arg arm_id_1)_finger_joint1" transmission="hardware_interface/EffortJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_1)_finger_joint2" transmission="hardware_interface/EffortJointInterface" />

    <!-- left hand joints control interface -->
    <xacro:gazebo-joint joint="$(arg arm_id_2)_finger_joint1" transmission="hardware_interface/EffortJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_2)_finger_joint2" transmission="hardware_interface/EffortJointInterface" />

    <!-- load ros_control plugin -->
    <gazebo>
        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"/>
        <robotSimType>franka_gazebo/FrankaHWSim</robotSimType>
    </gazebo>

</robot>

And this is my launch:


<?xml version="1.0"?>
<launch>

    <!-- Launch empty Gazebo world -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="use_sim_time" value="true" />
        <arg name="gui" value="true" />
        <arg name="paused" value="true" />
        <arg name="debug" value="false" />
    </include>

    <!-- Load the robot description file-->
    <param name="robot_description" command="$(find xacro)/xacro  '$(find franka_bimanual_controllers)/description/dual_panda_2.urdf.xacro'" />

    <!-- Spawn the robot over the robot_description param-->
    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" args="-urdf -param robot_description -model dual_panda" />

    <!-- Start the combined control node -->
    <rosparam command="load" file="$(find franka_bimanual_controllers)/config/franka_bimanual_controllers.yaml" />
    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="bimanual_cartesian_impedance_controller"/>
    <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" /> 

    <!-- Generate joint_states and tf_tree -->
   <!-- <node if="$(arg gui)" name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
    <node unless="$(arg gui)" name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
    <node name="rviz" pkg="rviz" type="rviz" required="true" /> -->

    <!--  Optional: Spawn the robot at specific pause use the below spawner args, and set the above paused arg to true. This initial pose should correspond to the initial pose in the robot srdf-->
    <!-- <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" args="-urdf -param robot_description -model panda_multiple_arms
    -J right_arm_joint1 0.0
    -J right_arm_joint2 -0.785
    -J right_arm_joint3 0.0
    -J right_arm_joint4 -2.356
    -J right_arm_joint5 0.0
    -J right_arm_joint6 1.571
    -J right_arm_joint7 0.785
    -J left_arm_joint1 0.0
    -J left_arm_joint2 -0.785
    -J left_arm_joint3 0.0
    -J left_arm_joint4 -2.356
    -J left_arm_joint5 0.0
    -J left_arm_joint6 1.571
    -J left_arm_joint7 0.785
    -unpause"/> -->

</launch>

When I try to launch in gazebo, I'm getting this warning during the launch:

[ WARN] [1712839920.741625034]: Transmission panda_right_franka_state has more than one joint. Currently the default robot hardware simulation  interface only supports one.
[ WARN] [1712839920.741645279]: Transmission panda_right_franka_model has more than one joint. Currently the default robot hardware simulation  interface only supports one.
[ WARN] [1712839920.741652543]: Transmission panda_left_franka_state has more than one joint. Currently the default robot hardware simulation  interface only supports one.
[ WARN] [1712839920.741658723]: Transmission panda_left_franka_model has more than one joint. Currently the default robot hardware simulation  interface only supports one.

After I press play in gazebo, the simulation crashes with this errors:

[ERROR] [1712839920.805649563]: This controller requires a hardware interface of type 'franka_hw::FrankaModelInterface', but is not exposed by the robot. Available interfaces in robot:
- 'hardware_interface::EffortJointInterface'
- 'hardware_interface::JointStateInterface'
- 'hardware_interface::PositionJointInterface'
- 'hardware_interface::VelocityJointInterface'
[ERROR] [1712839920.805682166]: Initializing controller 'bimanual_cartesian_impedance_controller' failed

I'm doing something wrong?

  1. First, of course that both pandas has more than one joint, how does the hardware simulation just support one?
  2. To expose franka_hw::FrankaModelInterface I added the ```

    <xacro:transmission-franka-model arm_id="$(arg arm_id_1)"

    
tags to my urdf.

I just dont get what wrong here. I also, found this issue where he relates the same warnings #313 but the issue is different