Trouble with JointEffortController & Gazebo
I'm having trouble getting my simulated robot arm to respond to effort commands. I've pretty much done everything according to the ros_control / gazebo tutorials, but whenever I manually publish to <controller_name>/command
, nothing happens.
Here's (hopefully) all of the info that is relevant. I've had this working with position control in the past, and I've changed it only slightly to try and get it to work with effort control. (Note, I've replaced the actual robot name with MYROBOT).
I'm using ROS indigo, and I built gazebo_ros_pkgs, ros_control, and ros_controllers from source (checking out the indigo-devel branch each time). I'm using gazebo2, installed from apt-get.
Selected elements from the xacro:
<robot name="MYROBOT" xmlns="...">
...
<link name="world" />
<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="base_link"/>
</joint>
...
<transmission name="elbow_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="elbow_joint">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="elbow_motor">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
...
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/MYROBOT</robotNamespace>
</plugin>
</gazebo>
</robot>
MYROBOT_control.yaml:
MYROBOT:
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
elbow_effort_controller:
type: effort_controllers/JointEffortController
joint: elbow_joint
pid: {p: 1.0, i: 0.01, d: 0.0}
MYROBOT_simulation.launch:
<include file="$(find gazebo_ros)/launch/empty_world.launch">
... (A bunch of <arg>s)
</include>
<param name="robot_description"
command="$(find xacro)/xacro.py '$(find MYROBOT_arm_description)/urdf/arm_geo.xacro'" />
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
respawn="false" output="screen" args="-urdf -model MYROBOT -param robot_description"/>
<rosparam file="$(find MYROBOT_ctrl)/config/MYROBOT_control.yaml" command="load"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner"
respawn="false" output="screen" ns="/MROBOT"
args="joint_state_controller elbow_effort_controller" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
respawn="false" output="screen">
<remap from="/joint_states" to="/MYROBOT/joint_states" />
</node>
Now, if I run this launch file, I don't see any obvious errors in the console; everything looks pretty healthy (No red text, just messages like "Loaded gazebo_ros_control", "Started controllers: joint_state_controller, elbow_effort_controller, ...", etc.). I see Gazebo with the spawned arm, but it sort of just sags and eventually lays flat out on the ground. I tried to publish a command to the elbow controller (for example) using rostopic pub /MYROBOT/elbow_effort_controller/command std_msgs/Float64 2.0
, but with no luck -- the model doesn't move. And if I echo /MYROBOT/joint_states
, the effort array is all zeros.
Is there something here that I'm doing incorrectly? Am I not using a controller correctly? Any help would be much appreciated!
EDIT 1: what the elbow joint looks like in the xacro
<joint
name="elbow_joint"
type="continuous">
<origin
xyz="-0.314325000000001 0 0"
rpy="-3.14159265358979 2.77555756156289E-16 1.67208086071471" />
<parent
link="upper_arm" />
<child
link="lower_arm" />
<axis
xyz="0 0 1" />
<limit
lower="1.57"
upper="-1.57"
effort="0"
velocity="0" />
<dynamics damping="0.7" />
</joint>