URDF model sink to the ground after joint start moving
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