Hi, I have a strange problem again and I can't figure out what is going wrong.
I use the gazebo_ros_controller_manager plugin for controlling an arm and a pan/tilt unit in gazebo simulation. I use similar configuration parameters for both of them. I posted the yaml files at the end. Both configuration files are loaded to the parameter server, then I use the pr2_controller_manager spawner node to load the controllers. It works perfectly when I load the arm_controller, I can move the arm and everything. But when I try to load the pantilt_controller, the simulation gets freaky. All the robot parts that are attached with non-fixed joints are displayed somewhere in the centre of the gazebo area, the published joint states look like this:
header:
seq: 131001
stamp:
secs: 1311
nsecs: 61000000
frame_id: ''
name: ['base_link_to_wheel_l_rim_joint', 'base_link_to_wheel_r_rim_joint', 'lenkrolle_base_to_lenkrolle_dreh_joint', 'lenkrolle_dreh_to_lenkrolle_rim_joint', 'kopf_base_to_kopf_pan_joint', 'kopf_pan_to_kopf_tilt_joint', 'arm_1_joint', 'arm_2_joint', 'arm_3_joint', 'arm_4_joint', 'arm_5_joint', 'arm_6_joint', 'arm_7_joint', 'gripper_base_to_finger_l', 'gripper_base_to_finger_r']
position: [31.415926535897945, 37.699111843077524, 9.42486522723199, 3.1416799200523933, -9.424716252608002, -9.424872208549377, -3.1415926535897913, -10.995570796905772, -1.5707998174534001, 7.853985124632986, 4.712385489726186, 1.5707998174534001, -1.5707998174533984, nan, nan]
velocity: [nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan]
effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, nan, nan]
All the position values should be close to 0, velocity too. The other joints are controlled by different controllers, they don't even use the pr2 controller_manager, except for publishing the joint states. I tried using the JointSplineTrajectoryController instead but with the same result. Same result if I don't start the arm controller.
I'm really lost. I have no idea what I am doing wrong. It used to work some time ago, but I haven't used the simulation for some time. I can't remember I changed anything since it worked. There are no error messages, the spawner just gives two info messages "Loaded controllers: pantilt_controller" and "Started controllers: pantilt_controller". Maybe it is just a simple but stupid mistake again, but I can't find it. Please help me solve this problem. Thanks.
The configuration files I use:
arm_configuration.yaml:
arm_controller:
type: robot_mechanism_controllers/JointTrajectoryActionController
joints:
- arm_1_joint
- arm_2_joint
- arm_3_joint
- arm_4_joint
- arm_5_joint
- arm_6_joint
- arm_7_joint
gains:
arm_1_joint:
p: 2000.0
d: 0.0
i: 100.0
arm_2_joint:
p: 2000.0
d: 0.0
i: 100.0
arm_3_joint:
p: 2000.0
d: 0.0
i: 100.0
arm_4_joint:
p: 2020.0
d: 0.0
i: 100.0
arm_5_joint:
p: 2030.0
d: 0.0
i: 100.0
arm_6_joint:
p: 5050.0
d: 0.0
i: 100.0
arm_7_joint:
p: 2080.0
d: 0.0
i: 100.0
arm_joint_trajectory_action_node:
joints:
- arm_1_joint
- arm_2_joint
- arm_3_joint
- arm_4_joint
- arm_5_joint
- arm_6_joint
- arm_7_joint
constraints:
goal_time: 0.6
arm_1_joint:
goal: 0.05
arm_2_joint:
goal: 0.05
arm_3_joint:
goal: 0.05
arm_4_joint:
goal: 0.05
arm_5_joint:
goal: 0.05
arm_6_joint:
goal: 0.05
arm_7_joint:
goal: 0.05
Update 1: I found out that the problem is caused by the kopf_pan_to_kopf_tilt_joint only. I tried to change the p,i,d parameters for the controller and I can change the behavior. When setting p=0.05,d=0,i=0.02 it seems to work mostly. The robot does not disappear and I can move the pan-tilt unit. But there are still problems. When I command to move beyond the limits the joint start bouncing. After executing one action server becomes unresponsive and I can't move the joints anymore. I have now installed ROS fuerte and I can't even load my model. But at least I get some error messages:
Error [Param.hh:223] Unable to set value [-nan -nan -nan 0 0 0] for key[pose]
Error [Param.hh:223] Unable to set value [-nan] for key[ixx]
Error [parser_deprecated.cc:437] one of the inertia elements: ixx (-nan) ixy (nan) ixz (-nan) iyy (-nan) iyz (-nan) izz (nan) is not a valid double.
Error [parser_deprecated.cc:647] Could not parse inertial element for Link 'kopf_tilt'
Error [parser_deprecated.cc:991] link xml is not initialized correctly
Error [parser_deprecated.cc:1048] model xml is not initialized correctly
Warning [parser.cc:275] parse as old deprecated world file failed, trying old model format.
Error [parser.cc:281] parse as old deprecated model file failed.
Error [World.cc:996] Unable to read sdf string
It seems there is something wrong with my model file, but I can't find where the problem is. The inertial element of kopf_tilt is a copy of the inertial in kopf_pan, so no idea why there is a parser error. The other error messages are not very specific, I'm not even sure if they are about the same link or not, but I guess so. I have added the relevant part of my urdf below. In fuerte it seems the gazebo interface has changed, it does not show the simulation window, and there isn't yet any documentation available. It looks like my model format has been replaced by something else and is deprecated, but I have no idea what the new format should look like. I want to continue using ROS electric, so I don't care if my model does only work in electric, but at least I'd like to know what is the problem with my model. I guess the problem is the same in electric but gazebo doesn't show error messages.
pan_tilt_configuration.yaml:
pantilt_controller:
type: robot_mechanism_controllers/JointTrajectoryActionController
joints:
- kopf_base_to_kopf_pan_joint
- kopf_pan_to_kopf_tilt_joint
gains:
kopf_base_to_kopf_pan_joint:
p: 500.0
d: 200.0
i: 200.0
kopf_pan_to_kopf_tilt_joint:
p: 500.0
d: 200.0
i: 200.0
pantilt_joint_trajectory_action_node:
joints:
- kopf_base_to_kopf_pan_joint
- kopf_pan_to_kopf_tilt_joint
constraints:
goal_time: 0.6
kopf_base_to_kopf_pan_joint:
goal: 0.05
kopf_pan_to_kopf_tilt_joint:
goal: 0.05
part of the urdf xacro file that defines the links and joints, the macro ${name} has the value "kopf" when loading the file <link name="${name}_pan">
</link>
<joint name="${name}_base_to_${name}_pan_joint" type="revolute" >
<origin xyz="0 0 0.0951" rpy="${pi} 0 0"/>
<parent link="${name}_base" />
<child link="${name}_pan" />
<axis xyz="0 0 1" />
<limit lower="${-pi}" upper="${pi}" effort="100" velocity="10" />
<calibration rising="0"/>
</joint>
<link name="${name}_tilt">
<visual>
<geometry>
<mesh filename="package://sr1_description/meshes/head/pw70_1.stl"/>
</geometry>
<origin rpy="${pi} 0 ${pi}" xyz="0 0 0"/>
<material name="Gold" />
</visual>
<collision>
<geometry>
<mesh filename="package://sr1_description/meshes/head/pw70_1.stl"/>
</geometry>
<origin rpy="${pi} 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="1.6"/>
<inertia ixx="0.10" ixy="0.0" ixz="0.0" iyy="0.10" iyz="0.0" izz="0.10"/>
</inertial>
</link>
<joint name="${name}_pan_to_${name}_tilt_joint" type="revolute" >
<origin xyz="0 0 0" rpy="0 0 ${pi}"/>
<parent link="${name}_pan" />
<child link="${name}_tilt" />
<axis xyz="1 0 0" />
<limit lower="${-pi/2}" upper="0.7" effort="10" velocity="1" />
<calibration rising="0"/>
</joint>
Originally posted by Kai Bublitz on ROS Answers with karma: 357 on 2012-04-18
Post score: 2
Original comments
Comment by bit-pirate on 2012-04-19:
Why do you use the d-term for the pan-tilt head and not for the arm? Could you try your simulation with setting the d-term for the "kopf" :-) to zero?
Comment by Kai Bublitz on 2012-04-19:
I have tried different settings for p,i,d. Setting d to zero alone does not change anything, but when I also set p and i to very small values (like 0.1) it works better. The robot does not "fly away" and kill the controllers but still acts strange. (I update the question)
Comment by phil0stine on 2012-08-22:
I am having an identical issue, have you come to a solution? Thanks
Comment by Kai Bublitz on 2012-08-23:
Unfortunately I was not able to find a solution. I think there is a problem with the urdf to gazebo converter, but I have no time to do further investigations. I will open another question on another issue I have with my model, perhaps this is somehow related.
Comment by hsu on 2012-08-23:
does it stabilize if all p,i,d are zeros?
Comment by phil0stine on 2012-08-23:
Wow yes that does the trick, at least for me. I had even lowered the values, but obviously I have poor intuition as to what values are reasonable. Anything less than 1 is working well. Thanks!!! Of course, I am receptive to reasons I should have known better :)
Comment by Kai Bublitz on 2012-08-27:
with all p,i,d zero the movable parts keep their initial orientation no matter how I move the robot. Of course the controller doesn't do anything.