0
$\begingroup$

I've created a workspace where I launch a panda robot with a custom controller (CartesianImpedance_trajectory_controller) from https://github.com/matthias-mayr/Cartesian-Impedance-Controller and I'm having some issues when launching both to gazebo. The problem I'm facing is that whenever I launch the robot and the controller without grouping them into a namespace, the simulation works and everything is fine. If I group them into a namespace called /panda, the simulation launches with any error but is not working.

This is my main.launch:

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

  <!-- Gazebo & GUI Configuration -->
  <arg name="debug" default="false"/>
  <arg name="gui" default="true"/>
  <arg name="paused" default="true"/>
  <arg name="world" default="$(find robot_ctrl)/world/worldTable.world"/>
  <arg name="iiwa_controllers" default="joint_state_controller CartesianImpedance_trajectory_controller"/>
  <arg name="panda_controllers"  default="CartesianImpedance_trajectory_controller" />
  <arg name="robot_name" default="iiwa" />
  <arg name="robot_state_frequency" default="100" />



   <!-- Robot Customization: PANDA -->
   <include file="$(find robot_ctrl)/launch/launch_panda.launch">
    <arg name="x" value="0.6"/>
    <arg name="y" value="0"/>
    <arg name="z" value="0.5"/>
    <arg name="roll" value="0"/>
    <arg name="pitch" value="0"/>
    <arg name="yaw" value="3.1415"/>
    <arg name="paused" value="$(arg paused)"/>
   </include>
   

  
  <!-- Robot Customization: iiwa 
   <include file="$(find robot_ctrl)/launch/launch_iiwa.launch">
    <arg name="origin_xyz" default="'-0.6 0 0.5'"/> 
    <arg name="origin_rpy" default="'0 0 0'"/>
   </include> -->

   <!-- World launch -->
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(arg world)"/>
    <arg name="debug" value="$(arg debug)"/>
    <arg name="paused" value="$(arg paused)"/>
    <arg name="gui" value="$(arg gui)"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="headless"    default="false" />
   </include>
   
</launch>

this is my panda launch (with the group commented out):

<?xml version="1.0"?>
 
 <launch>
 <!-- Robot Customization PANDA -->
  <arg name="robot"       default="panda"                doc="Which robot to spawn (one of {panda,fr3})" />
  <arg name="arm_id"      default="$(arg robot)" doc="Name of the robot to spawn" />
  <arg name="use_gripper" default="true"  doc="Should a franka hand be mounted on the flange?" />
  <arg name="x"           default="0.6"     doc="How far forward to place the base of the robot in [m]?" />
  <arg name="y"           default="0"     doc="How far leftwards to place the base of the robot in [m]?" />
  <arg name="z"           default="0.5"     doc="How far upwards to place the base of the robot in [m]?" />
  <arg name="roll"        default="0"     doc="How much to rotate the base of the robot around its X-axis in [rad]?" />
  <arg name="pitch"       default="0"     doc="How much to rotate the base of the robot around its Y-axis in [rad]?" />
  <arg name="yaw"         default="3.1415"     doc="How much to rotate the base of the robot around its Z-axis in [rad]?" />
  <arg name="xacro_args"  default=""      doc="Additional arguments to pass to panda.urdf.xacro" />
  <arg name="paused" default="true"/>
  <arg name="initial_joint_positions"

       default="-J $(arg arm_id)_joint1 0
                -J $(arg arm_id)_joint2 -0.785398163
                -J $(arg arm_id)_joint3 0
                -J $(arg arm_id)_joint4 -2.35619449
                -J $(arg arm_id)_joint5 0
                -J $(arg arm_id)_joint6 1.57079632679
                -J $(arg arm_id)_joint7 0.785398163397
                -J $(arg arm_id)_finger_joint1 0.001
                -J $(arg arm_id)_finger_joint2 0.001"
       />
       

    <arg name="panda_controllers"  default="/panda/CartesianImpedance_trajectory_controller" />
    
     <!-- Spawn required ROS controllers -->
     <node pkg="controller_manager" type="spawner" name="Cartesian_controller_spawner" output="screen" args="$(arg panda_controllers) " respawn="false"/>
  
   <!-- <group ns="panda" >-->

      <rosparam file="$(find franka_gazebo)/config/franka_hw_sim.yaml" subst_value="true" />
      <rosparam file="$(find robot_ctrl)/config/panda_controller.yaml" subst_value="true"/>
      <rosparam file="$(find robot_ctrl)/config/controllerPanda.yaml" subst_value="true"/>
      
      <param name="m_ee" value="0.76" if="$(arg use_gripper)" />
      
      <param name="robot_description"
         command="xacro $(find franka_description)/robots/$(arg robot)/$(arg robot).urdf.xacro
                  gazebo:=true
                  hand:=$(arg use_gripper)
                  arm_id:=$(arg arm_id)
                  xyz:='$(arg x) $(arg y) $(arg z)'
                  rpy:='$(arg roll) $(arg pitch) $(arg yaw)'
                  $(arg xacro_args)">
      </param>

      <!-- Run a python script to send a service call to gazebo_ros to spawn a URDF robot -->
      <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model $(arg arm_id) $(arg initial_joint_positions) "/>
      

      <!-- Spawn required ROS controllers -->
      <node pkg="controller_manager" type="spawner" name="$(arg arm_id)_gripper_spawner" if="$(arg use_gripper)" args="franka_gripper" respawn="false"/>
      
      <!-- Panda Controller-->
       <node pkg="controller_manager"
        type="spawner"
        name="panda_controller_spawner"
        respawn="false" output="screen"
        args="--wait-for initialized franka_state_controller "
       />
      
      <!-- Panda state publisher -->
      <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>
      
    <!--</group>-->
      
</launch>

my yaml:

franka_state_controller:
  type: franka_control/FrankaStateController
  publish_rate: 50  # [Hz]
  arm_id: $(arg arm_id)
  joint_names:
    - $(arg arm_id)_joint1
    - $(arg arm_id)_joint2
    - $(arg arm_id)_joint3
    - $(arg arm_id)_joint4
    - $(arg arm_id)_joint5
    - $(arg arm_id)_joint6
    - $(arg arm_id)_joint7
    
CartesianImpedance_trajectory_controller:
  type: cartesian_impedance_controller/CartesianImpedanceController
  arm_id: $(arg arm_id)
  joints:
    - $(arg arm_id)_joint1
    - $(arg arm_id)_joint2
    - $(arg arm_id)_joint3
    - $(arg arm_id)_joint4
    - $(arg arm_id)_joint5
    - $(arg arm_id)_joint6
    - $(arg arm_id)_joint7
  end_effector: panda_hand_tcp            # Link to control arm in
  update_frequency: 500                 # Controller update frequency in Hz
  # Optional parameters - the mentioned values are the defaults
  dynamic_reconfigure: true             # Starts dynamic reconfigure server
  handle_trajectories: true             # Accept traj., e.g. from MoveIt
  robot_description: /robot_description # In case of a varying name
  wrench_ee_frame: panda_hand_tcp         # Default frame for wrench commands
  delta_tau_max: 1.0                    # Max. commanded torque diff between steps in Nm
  filtering:                            # Update existing values (0.0 1.0] per s
    nullspace_config: 0.1               # Nullspace configuration filtering
    pose: 0.1                           # Reference pose filtering
    stiffness: 0.1                      # Cartesian and nullspace stiffness
    wrench: 0.1                         # Commanded torque
  verbosity:
    verbose_print: false                # Enables additional prints
    state_msgs: false                   # Messages of controller state
    tf_frames: false                    # Extra tf frames

model_example_controller:
  type: franka_example_controllers/ModelExampleController
  arm_id: $(arg arm_id)

force_example_controller:
  type: franka_example_controllers/ForceExampleController
  arm_id: $(arg arm_id)
  joint_names:
    - $(arg arm_id)_joint1
    - $(arg arm_id)_joint2
    - $(arg arm_id)_joint3
    - $(arg arm_id)_joint4
    - $(arg arm_id)_joint5
    - $(arg arm_id)_joint6
    - $(arg arm_id)_joint7

cartesian_impedance_example_controller:
  type: franka_example_controllers/CartesianImpedanceExampleController
  arm_id: $(arg arm_id)
  joint_names:
    - $(arg arm_id)_joint1
    - $(arg arm_id)_joint2
    - $(arg arm_id)_joint3
    - $(arg arm_id)_joint4
    - $(arg arm_id)_joint5
    - $(arg arm_id)_joint6
    - $(arg arm_id)_joint7

effort_joint_trajectory_controller:
  type: effort_controllers/JointTrajectoryController
  joints:
    - $(arg arm_id)_joint1
    - $(arg arm_id)_joint2
    - $(arg arm_id)_joint3
    - $(arg arm_id)_joint4
    - $(arg arm_id)_joint5
    - $(arg arm_id)_joint6
    - $(arg arm_id)_joint7
  gains:
    $(arg arm_id)_joint1: { p: 600, d: 30, i: 0 }
    $(arg arm_id)_joint2: { p: 600, d: 30, i: 0 }
    $(arg arm_id)_joint3: { p: 600, d: 30, i: 0 }
    $(arg arm_id)_joint4: { p: 600, d: 30, i: 0 }
    $(arg arm_id)_joint5: { p: 250, d: 10, i: 0 }
    $(arg arm_id)_joint6: { p: 150, d: 10, i: 0 }
    $(arg arm_id)_joint7: { p: 50, d: 5, i: 0 }
  constraints:
    goal_time: 0.5
    $(arg arm_id)_joint1: { goal: 0.05 }
    $(arg arm_id)_joint2: { goal: 0.05 }
    $(arg arm_id)_joint3: { goal: 0.05 }
    $(arg arm_id)_joint4: { goal: 0.05 }
    $(arg arm_id)_joint5: { goal: 0.05 }
    $(arg arm_id)_joint6: { goal: 0.05 }
    $(arg arm_id)_joint7: { goal: 0.05 }

position_joint_trajectory_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - $(arg arm_id)_joint1
    - $(arg arm_id)_joint2
    - $(arg arm_id)_joint3
    - $(arg arm_id)_joint4
    - $(arg arm_id)_joint5
    - $(arg arm_id)_joint6
    - $(arg arm_id)_joint7
  constraints:
    goal_time: 0.5
    $(arg arm_id)_joint1: { goal: 0.05}
    $(arg arm_id)_joint2: { goal: 0.05}
    $(arg arm_id)_joint3: { goal: 0.05}
    $(arg arm_id)_joint4: { goal: 0.05}
    $(arg arm_id)_joint5: { goal: 0.05}
    $(arg arm_id)_joint6: { goal: 0.05}
    $(arg arm_id)_joint7: { goal: 0.05}

joint_velocity_example_controller:
    type: franka_example_controllers/JointVelocityExampleController
    joint_names:
        - $(arg arm_id)_joint1
        - $(arg arm_id)_joint2
        - $(arg arm_id)_joint3
        - $(arg arm_id)_joint4
        - $(arg arm_id)_joint5
        - $(arg arm_id)_joint6
        - $(arg arm_id)_joint7

joint_position_example_controller:
    type: franka_example_controllers/JointPositionExampleController
    joint_names:
        - $(arg arm_id)_joint1
        - $(arg arm_id)_joint2
        - $(arg arm_id)_joint3
        - $(arg arm_id)_joint4
        - $(arg arm_id)_joint5
        - $(arg arm_id)_joint6
        - $(arg arm_id)_joint7

The difference between the graph is the following: With Ns enter image description here

without Ns: enter image description here

And after launching, I tried to do a rostopic list. There is no difference between them further than the namespace /panda before the topics.

enter image description here

I dont understand why when launching without the namespace I do for example a rostopic echo /CartesianImpedance_trajectory_controller/commanded_torques and I can see all the information. But when I do the same when I have the namespace rostopic echo /panda/CartesianImpedance_trajectory_controller/commanded_torques there is no information published.

I'm missing something with the namespaces but I need them in order to add another robot and separate both in two different namespaces.

Thanks in advance and regards.

$\endgroup$

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.