search

frankaemika / franka_ros

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

gazebo doesnt find joints when uploading two panda robots #372

Open mikelasa opened 1 year ago

mikelasa commented 1 year ago

Hello, I have been following the moveit 1 tutorials for creating 2 panda robots and I found out that when trying to launch both pandas in gazebo this error appears: 

Joint 'panda1_joint1' contains a 'transmission_interface/SimpleTransmission' transmission, but it's not part of the Franka robot. Ignoring this joint in FrankaHWSim
[ WARN] [1696614862.317198707, 0.369000000]: Joint 'panda1_joint2' contains a 'transmission_interface/SimpleTransmission' transmission, but it's not part of the Franka robot. Ignoring this joint in FrankaHWSim
[ WARN] [1696614862.317214667, 0.369000000]: Joint 'panda1_joint3' contains a 'transmission_interface/SimpleTransmission' transmission, but it's not part of the Franka robot. Ignoring this joint in FrankaHWSim
[ WARN] [1696614862.317227071, 0.369000000]: Joint 'panda1_joint4' contains a 'transmission_interface/SimpleTransmission' transmission, but it's not part of the Franka robot. Ignoring this joint in FrankaHWSim
[ WARN] [1696614862.317237675, 0.369000000]: Joint 'panda1_joint5' contains a 'transmission_interface/SimpleTransmission' transmission, but it's not part of the Franka robot. Ignoring this joint in FrankaHWSim
[ WARN] [1696614862.317248045, 0.369000000]: Joint 'panda1_joint6' contains a 'transmission_interface/SimpleTransmission' transmission, but it's not part of the Franka robot. Ignoring this joint in FrankaHWSim
[ WARN] [1696614862.317257900, 0.369000000]: Joint 'panda1_joint7' contains a 'transmission_interface/SimpleTransmission' transmission, but it's not part of the Franka robot. Ignoring this joint in FrankaHWSim
[ WARN] [1696614862.317268095, 0.369000000]: Joint 'panda2_joint1' contains a 'transmission_interface/SimpleTransmission' transmission, but it's not part of the Franka robot. Ignoring this joint in FrankaHWSim
[ WARN] [1696614862.317278045, 0.369000000]: Joint 'panda2_joint2' contains a 'transmission_interface/SimpleTransmission' transmission, but it's not part of the Franka robot. Ignoring this joint in FrankaHWSim
[ WARN] [1696614862.317290787, 0.369000000]: Joint 'panda2_joint3' contains a 'transmission_interface/SimpleTransmission' transmission, but it's not part of the Franka robot. Ignoring this joint in FrankaHWSim
[ WARN] [1696614862.317304170, 0.369000000]: Joint 'panda2_joint4' contains a 'transmission_interface/SimpleTransmission' transmission, but it's not part of the Franka robot. Ignoring this joint in FrankaHWSim
[ WARN] [1696614862.317314891, 0.369000000]: Joint 'panda2_joint5' contains a 'transmission_interface/SimpleTransmission' transmission, but it's not part of the Franka robot. Ignoring this joint in FrankaHWSim
[ WARN] [1696614862.317325591, 0.369000000]: Joint 'panda2_joint6' contains a 'transmission_interface/SimpleTransmission' transmission, but it's not part of the Franka robot. Ignoring this joint in FrankaHWSim
[ WARN] [1696614862.317335771, 0.369000000]: Joint 'panda2_joint7' contains a 'transmission_interface/SimpleTransmission' transmission, but it's not part of the Franka robot. Ignoring this joint in FrankaHWSim
[ WARN] [1696614862.317346521, 0.369000000]: Joint 'panda1_finger_joint1' contains a 'transmission_interface/SimpleTransmission' transmission, but it's not part of the Franka robot. Ignoring this joint in FrankaHWSim
[ WARN] [1696614862.317357008, 0.369000000]: Joint 'panda1_finger_joint2' contains a 'transmission_interface/SimpleTransmission' transmission, but it's not part of the Franka robot. Ignoring this joint in FrankaHWSim
[ WARN] [1696614862.317369571, 0.369000000]: Joint 'panda2_finger_joint1' contains a 'transmission_interface/SimpleTransmission' transmission, but it's not part of the Franka robot. Ignoring this joint in FrankaHWSim
[ WARN] [1696614862.317381296, 0.369000000]: Joint 'panda2_finger_joint2' contains a 'transmission_interface/SimpleTransmission' transmission, but it's not part of the Franka robot. Ignoring this joint in FrankaHWSim

I followed this tutorial: https://ros-planning.github.io/moveit_tutorials/doc/multiple_robot_arms/multiple_robot_arms_tutorial.html

I triple checked my xacro file and my configuration files too and so far, I dont see any difference betwen the names of the joints. I have read as well a similar problem that someone had in this forum:

https://github.com/frankaemika/franka_ros/issues/313

but I dont know how to fix it. My files are located like this:

-franka_ros
        - dual_panda_moveit_config
                -config
                -launch 
        - dual_impedance_panda
                -robots (my urdf is here)

Here is my xacro file: 

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

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

    <!-- 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">
    </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 0" 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 0" 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/PositionJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_1)_joint2" transmission="hardware_interface/PositionJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_1)_joint3" transmission="hardware_interface/PositionJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_1)_joint4" transmission="hardware_interface/PositionJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_1)_joint5" transmission="hardware_interface/PositionJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_1)_joint6" transmission="hardware_interface/PositionJointInterface" />
    <xacro:gazebo-joint joint="$(arg arm_id_1)_joint7" transmission="hardware_interface/PositionJointInterface" />

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

    <!-- 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">
          <controlPeriod>0.001</controlPeriod>
          <robotSimType>franka_gazebo/FrankaHWSim</robotSimType>
        </plugin>
        <self_collide>true</self_collide>
      </gazebo>

</robot>

and here my yaml files, first gazebo yaml:

# Publish joint_states
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50

my dual_panda_controllers.yaml

panda1_arm_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - panda1_joint1
    - panda1_joint2
    - panda1_joint3
    - panda1_joint4
    - panda1_joint5
    - panda1_joint6
    - panda1_joint7
  constraints:
    goal_time: 0.6
    stopped_velocity_tolerance: 0.05
    panda1_joint1: {trajectory: 0.1, goal: 0.1}
    panda1_joint2: {trajectory: 0.1, goal: 0.1}
    panda1_joint3: {trajectory: 0.1, goal: 0.1}
    panda1_joint4: {trajectory: 0.1, goal: 0.1}
    panda1_joint5: {trajectory: 0.1, goal: 0.1}
    panda1_joint6: {trajectory: 0.1, goal: 0.1}
    panda1_joint7: {trajectory: 0.1, goal: 0.1}
  stop_trajectory_duration: 0.5
  state_publish_rate: 25
  action_monitor_rate: 10

panda2_arm_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - panda2_joint1
    - panda2_joint2
    - panda2_joint3
    - panda2_joint4
    - panda2_joint5
    - panda2_joint6
    - panda2_joint7
  constraints:
    goal_time: 0.6
    stopped_velocity_tolerance: 0.05
    panda2_joint1: {trajectory: 0.1, goal: 0.1}
    panda2_joint2: {trajectory: 0.1, goal: 0.1}
    panda2_joint3: {trajectory: 0.1, goal: 0.1}
    panda2_joint4: {trajectory: 0.1, goal: 0.1}
    panda2_joint5: {trajectory: 0.1, goal: 0.1}
    panda2_joint6: {trajectory: 0.1, goal: 0.1}
    panda2_joint7: {trajectory: 0.1, goal: 0.1}
  stop_trajectory_duration: 0.5
  state_publish_rate: 25
  action_monitor_rate: 10

panda1_hand_controller:
  type: effort_controllers/JointTrajectoryController
  joints:
    - panda1_finger_joint1
  gains:
    panda1_finger_joint1:
      p: 100
      d: 1
      i: 1
      i_clamp: 1

panda2_hand_controller:
  type: effort_controllers/JointTrajectoryController
  joints:
    - panda2_finger_joint1
  gains:
    panda2_finger_joint1:
      p: 100
      d: 1
      i: 1
      i_clamp: 1

and my launch files, control utils.launch:

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

<!-- Robot state publisher -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
    <param name="publish_frequency" type="double" value="50.0" />
    <param name="tf_prefix" type="string" value="" />
</node>

<!-- Joint state controller -->
<rosparam file="$(find dual_panda_moveit_config)/config/gazebo_controllers.yaml" command="load" />
<node name="joint_state_controller_spawner" pkg="controller_manager" type="spawner" args="joint_state_controller" respawn="false" output="screen" />

<!-- Joint trajectory controller -->
<rosparam file="$(find dual_panda_moveit_config)/config/traj_controller.yaml" command="load" />
<node name="arms_trajectory_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda1_arm_controller panda2_arm_controller panda1_hand_controller panda2_hand_controller" />

</launch>

empty_world.launch

<?xml version="1.0"?>
<launch>
    <arg name="robot"       default="panda" />
    <arg name="arm_id"      default="$(arg robot)" doc="Name of the robot to spawn" />
    <!-- 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="false" />
        <arg name="debug" value="false" />
    </include>

    <rosparam file="$(find franka_gazebo)/config/franka_hw_sim.yaml" subst_value="true" />
    <rosparam file="$(find franka_gazebo)/config/sim_controllers.yaml" subst_value="true" />

    <!-- Find my robot Description-->
    <param name="robot_description" command="$(find xacro)/xacro  '$(find dual_impedance_panda)/robots/dual_panda_arms.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_arms" />
</launch>

and launch_all.launch

<?xml version="1.0"?>
<launch>
    <!-- Run the main MoveIt executable with trajectory execution -->
    <include file="$(find dual_panda_moveit_config)/launch/move_group.launch">
        <arg name="allow_trajectory_execution" value="true" />
        <arg name="moveit_controller_manager" value="ros_control" />
        <arg name="fake_execution_type" value="interpolate" />
        <arg name="info" value="true" />
        <arg name="debug" value="false" />
        <arg name="pipeline" value="ompl" />
        <arg name="load_robot_description" value="true" />
    </include>

    <!-- Start the simulated robot in an empty Gazebo world -->
    <include file="$(find dual_panda_moveit_config)/launch/empty_world.launch" />

    <!-- Start the controllers and robot state publisher-->
    <include file="$(find dual_panda_moveit_config)/launch/control_utils.launch"/>

    <!-- Start moveit_rviz with the motion planning plugin -->
    <include file="$(find dual_panda_moveit_config)/launch/moveit_rviz.launch">
        <arg name="rviz_config" value="$(find dual_panda_moveit_config)/launch/moveit.rviz" />
    </include>

</launch>
SteMuc commented 8 months ago

Did you solve the problem? I have the same issue more or less. I would like to create the simulation of the dual arm Franka robot by using the Franka State Controller and not the joint state controller. :(