0
$\begingroup$

Rosanswers logo

I see that effort_controllers/JointPositionController looks for the particular joint and assigns the corresponding PID value to the control_toolbox::Pid variable. effort_controllers/JointGroupPositionController also does the same thing for all the joints inside the controller in a for loop. So essentially there is no difference between the two except for in the .yaml file and the launch files.

When I try to load a effort_controllers/JointGroupPositionController with the following .yaml file and launch file, I am getting the following error: "Failed to getParam 'joints' (namespace: /my_6dof_robot/joint_group_position_controller)".

[INFO] [1572904736.013589, 0.511000]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1572904736.015633, 0.512000]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1572904736.017165, 0.513000]: Loading controller: joint_group_position_controller
[ERROR] [1572904736.025517675, 0.519000000]: Failed to getParam 'joints' (namespace: /my_6dof_robot/joint_group_position_controller).
[ERROR] [1572904736.025622082, 0.519000000]: Failed to initialize the controller
[ERROR] [1572904736.025671797, 0.519000000]: Initializing controller 'joint_group_position_controller' failed
[ERROR] [1572904737.027262, 1.296000]: Failed to load joint_group_position_controller
[INFO] [1572904737.027650, 1.296000]: Loading controller: joint_state_controller
[INFO] [1572904737.035878, 1.303000]: Controller Spawner: Loaded controllers: joint_state_controller
[INFO] [1572904737.045714, 1.309000]: Started controllers: joint_state_controller

It succeeds in loading the joint_state_controller.

The .yaml file (my_6dof_robot_effort_control.yaml) is as follows:

my_6dof_robot:
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50
  joint_group_position_controller:
    type: effort_controllers/JointGroupPositionController
    joints:
      joint_base_link1: 
        pid: {p: 600.0, i: 80.0, d: 200.0}
      joint_link1_link2: 
        pid: {p: 11000.0, i: 100.0, d: 500.0}
      joint_link2_link3: 
        pid: {p: 8000.0, i: 300.0, d: 400.0}
      joint_link3_link4: 
        pid: {p: 400.0, i: 40.0, d: 60.0}
      joint_link4_link5: 
        pid: {p: 400.0, i: 10.0, d: 10.0}
      joint_link5_link6: 
        pid: {p: 20.0, i: 0.8, d: 0.1}

The gazebo and control launch file is as follows:

<launch>

  <arg name="paused" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>

  <include
    file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="headless" value="$(arg headless)"/>
  </include>

    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find my_6dof_robot_description)/urdf/my_6dof_robot_robot.xacro"/>
    <node name="spawn_my_6dof_robot" pkg="gazebo_ros" type="spawn_model" 
     args="-param robot_description
           -urdf 
           -x 0  
           -y 0
           -z 0
           -model my_6dof_robot"
     respawn="false" output="screen">
    </node>

    <rosparam file="$(find my_6dof_robot_control)/config/my_6dof_robot_effort_control.yaml" command="load" />

   
    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" ns="/my_6dof_robot_robot"
      output="screen" args="joint_group_position_controller joint_state_controller --timeout 50">  
    </node>
    <node name = "robot_state_publisher" pkg = "robot_state_publisher" type = "robot_state_publisher" respawn="false" output="screen">
      <remap from="/joint_states" to="/my_6dof_robot_robot/joint_states" />
    </node>
</launch>

However, when I load the effort_controllers/JointPositionController with the following .yaml and launch file, it is a success.

my_6dof__robot:
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50
  joint1_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint_base_link1
    pid: {p: 600.0, i: 80.0, d: 200.0}
  joint2_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint_link1_link2
    pid: {p: 11000.0, i: 100.0, d: 500.0}
  joint3_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint_link2_link3
    pid: {p: 8000.0, i: 300.0, d: 400.0}
  joint4_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint_link3_link4
    pid: {p: 400.0, i: 40.0, d: 60.0}
  joint5_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint_link4_link5
    pid: {p: 400.0, i: 10.0, d: 10.0}
  joint6_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint_link5_link6
    pid: {p: 20.0, i: 0.8, d: 0.1}

.launch file:

  <launch>
    
      <arg name="paused" default="false"/>
      <arg name="use_sim_time" default="true"/>
      <arg name="gui" default="true"/>
      <arg name="headless" default="false"/>
      <arg name="debug" default="false"/>
    
      <include
        file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="debug" value="$(arg debug)" />
        <arg name="gui" value="$(arg gui)" />
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
      </include>
    
        <param name="robot_description" command="$(find xacro)/xacro --inorder $(find my_6dof_robot_description)/urdf/my_6dof_robot_robot.xacro"/>
        <node name="spawn_my_6dof_robot" pkg="gazebo_ros" type="spawn_model" 
         args="-param robot_description
               -urdf 
               -x 0  
               -y 0
               -z 0
               -model my_6dof_robot"
         respawn="false" output="screen">
        </node>
    
        <rosparam file="$(find my_6dof_robot_control)/config/my_6dof_robot_effort_control.yaml" command="load" />
    
       
        <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" ns="/my_6dof_robot_robot"
          output="screen" args="joint1_position_controller joint2_position_controller joint3_position_controller joint4_position_controller joint5_position_controller joint6_position_controller joint_state_controller joint_state_controller --timeout 50">  
        </node>
        <node name = "robot_state_publisher" pkg = "robot_state_publisher" type = "robot_state_publisher" respawn="false" output="screen">
          <remap from="/joint_states" to="/my_6dof_robot_robot/joint_states" />
        </node>
    </launch>

The my_6dof_robot.urdf is has the following gazebo_ros_plugin tag in both the cases:

<gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
    <robotNamespace>/my_6dof_robot</robotNamespace>
    <robotParam>robot_description</robotParam>
    <legacyModeNS>true</legacyModeNS>
    </plugin>
</gazebo>

I think the error has something to do with the namespace. I want to know what Iam doing wrong. The same error is appearing when I try to load my custom controller instead of the joint_group_position_controller. Any help is highly appreciated.

Thank you!!


Originally posted by sthakar on ROS Answers with karma: 1 on 2019-10-29

Post score: 0


Original comments

Comment by gvdhoorn on 2019-10-29:
Please show a verbatim copy of the actual (and complete) error message.

Comment by sthakar on 2019-10-30:
Thanks! I have edited the question. Please let me know if you need any more information. Thank you!

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

I think the issue is that your parameters are not correct for the effort_controllers/JointGroupPositionController controller type. In general, it's currently a bit tough to find documentation on what parameters are required by each different ros_controllers controller -- I often find it's easiest to look at the source code. If you look at that previous link, you'll see they are expecting a param named joints that can be converted to a std::vector< std::string > (this is currently missing). They also initialize a PID Controller for each joint by passing a node handle constructed as ros::NodeHandle(n, joint_name + "/pid")).

So, maybe if your file looked something like the following (untested):

my_6dof_robot:
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50
  joint_group_position_controller:
    type: effort_controllers/JointGroupPositionController
    joints:
      - joint_base_link1
      - joint_link1_link2
      - joint_link2_link3
      - joint_link3_link4
      - joint_link4_link5
      - joint_link5_link6
    joint_base_link1: 
      pid: {p: 600.0, i: 80.0, d: 200.0}
    joint_link1_link2: 
      pid: {p: 11000.0, i: 100.0, d: 500.0}
    joint_link2_link3: 
      pid: {p: 8000.0, i: 300.0, d: 400.0}
    joint_link3_link4: 
      pid: {p: 400.0, i: 40.0, d: 60.0}
    joint_link4_link5: 
      pid: {p: 400.0, i: 10.0, d: 10.0}
    joint_link5_link6: 
      pid: {p: 20.0, i: 0.8, d: 0.1}

Originally posted by jarvisschultz with karma: 9031 on 2019-11-04

This answer was ACCEPTED on the original site

Post score: 3


Original comments

Comment by sthakar on 2019-11-04:
This actually works! Thanks a ton for your reply!! I went through the source code you pointed towards and now understand how it works! Appreciate your help!

Comment by jarvisschultz on 2019-11-05:
Awesome! I didn't have the time to test what I posted, so I'm glad it worked.

$\endgroup$

Your Answer

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