p3d controller not working properly in groovy ?

edit

I have a the model of a door in gazebo whose position I monitor using the p3d plugin. My algorithm for opening and closing the door is this

• Find and set the yaw at which door can be assumed to be open/close.
• Using the p3d controller get thecurrent yaw of the door joint (joint between door block and world).
• Until the current yaw = goal yaw apply a force to open/close the door. Once reached, remove the force and kill the node.

This was perfectly working for me in fuerte gazebo (1.0.2). I recently moved all my gazebo stuff to groovy. Now, by this algorithm, the doors swings and opens but some problem occurs and the door model seems to vibrate in place (shakes) and the node does not kill itself. I am suspecting it could be a problem with the p3d controller, since the node performs perfectly well in fuerte. I could also be in the urdf file. This is my urdf.xacro

<?xml version="1.0" ?>
<robot name="${room_name}_door" xmlns:xacro="http://ros.org/wiki/xacro" xmlns:xi="http://www.w3.org/2001/XInclude" xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics">
    <property name="room_name" value="workroom_f0" />
    <property name="x_position" value="3.73" />
    <property name="y_position" value="7.1" />
    <property name="z_position" value="0.06" />
    <property name="x_world_offset" value="-5" />
    <property name="y_world_offset" value="-5" />
    <property name="lower_limit" value="-2.5" />
    <property name="orientation" value="1.57" />
    <joint name="${room_name}_door_joint" type="revolute">
        <axis xyz="0 0 1" />
        <parent link="world" />
        <child link="${room_name}_door_block" />
        <origin rpy="0 0 ${orientation}" xyz="${x_position + x_world_offset} ${y_position + y_world_offset} ${z_position}" />
        <limit effort="100.0" lower="${lower_limit}" upper="0.001" velocity="5" />
    </joint>
    <joint name="doorblock_door" type="fixed">
        <axis xyz="0 0 1" />
        <parent link="${room_name}_door_block" />
        <child link="${room_name}_door" />
        <origin rpy="0 0 0" xyz="0 0 0" />
    </joint>
    <link name="world" />
    <link name="${room_name}_door_block">
        <inertial>
            <mass value="1" />
            <origin xyz="0 0 0" />
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1" iyz="0.0" izz="3" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <box size="0 0 0" />
            </geometry>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <box size="0 0 0" />
            </geometry>
        </collision>
    </link>
    <gazebo reference="${room_name}_door_block">
        <material>angen_gazebo/DoorWood</material>
        <laserRetro>2000</laserRetro>
        <dampingFactor>0.006</dampingFactor>
        <selfCollide>false</selfCollide>
        <turnGravityOff>true</turnGravityOff>
    </gazebo>
    <link name="${room_name}_door ...