Robotics StackExchange | Archived questions

URDF model sink to the ground after joint start moving edit

Hi, I'm using Gazebo 9. I tried to use ros_control package to control joint velocity. But as soon as I sent:

rostopic pub -1 /rover/left_wheel/command std_msgs/Float64 "data: -1"

or try to manually move the model, the model sinked to the ground and two of the wheels gone! I suspect something is wrong with my yaml setup. Here it is:

rover:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  

  # Position Controllers ---------------------------------------
  left_wheel:
    type: effort_controllers/JointVelocityController
    joint: holder2wheel_left
    pid: {p: 100.0, i: 0.01, d: 10.0}

  right_wheel:
    type: effort_controllers/JointVelocityController
    joint: holder2wheel_right
    pid: {p: 100.0, i: 0.01, d: 10.0}

And here's the launch file:

<launch>

  <param name="robot_description" command="cat $(find rover_project)/rover_description/urdf/rover.urdf"/>
  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find rover_project)/rover_description/config/rover_control.yaml" command="load"/>

  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/rover" args="left_wheel right_wheel joint_state_controller"/>

  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
    respawn="false" output="screen">
    <remap from="/joint_states" to="/rover/joint_states" />
  </node>

</launch>

And here's the transmission part in my urdf file:

  <transmission name="tran1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="holder2wheel_left">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_left">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <transmission name="tran2">

    <type>transmission_interface/SimpleTransmission</type>
    <joint name="holder2wheel_right">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_right">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>

  </transmission>

  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/rover</robotNamespace>
    </plugin>
  </gazebo>

Any idea?

Before: https://imgur.com/R8Bpsnk

After: https://imgur.com/alcDCzl

Asked by vitsensei on 2019-11-06 00:52:40 UTC

Comments

Answers

Hello, was your robot working without the ros_control packge? Could you post the URDF from your robot? I think you are following this tutorial, right? Maybe you are missing a single quote on the joint's name:

# Position Controllers ---------------------------------------
left_wheel:
    type: effort_controllers/JointVelocityController
    joint: 'holder2wheel_left'
    pid: {p: 100.0, i: 0.01, d: 10.0}

Even if you get it to work, I suggest to use the diff_drive_controller. It seems more adequate for a rover robot.

Asked by schulze_18 on 2019-11-09 19:23:02 UTC

Comments