Maintaining Fixed Position With Velocity Controller

edit

I've implemented a velocity controller for UR5. The issue I'm having is that I can't keep the robot at a fixed posture. Sending zero velocity makes the robot to be pulled downward by the gravity. Even for simple example as shown below, once I start the simulation, the arm falls down. My question is is it possible to maintain a fixed position with velocity controller?

urdf folder robot.xacro, robot.gazebo, materials.xacro

robot.xacro<robot <a="" href="http://xmlns:xacro="http://www.ros.org/wiki/xacro">xmlns:xacro="http://www.ros.org/wiki/..." name="rrbot"> <xacro:property name="width" value="0.1"> <xacro:property name="height1" value="1.5"> <xacro:property name="height2" value="1"> <xacro:property name="axle_offset" value="0.05"> <xacro:property name="damp" value="0.7"></xacro:property></xacro:property></xacro:property></xacro:property></xacro:property></robot>

    <xacro:macro name="default_inertial" params="z_value i_value mass">
        <inertial>
            <origin xyz="0 0 ${z_value}" rpy="0 0 0"/>
            <mass value="${mass}"/>
            <inertia  ixx="${i_value}" ixy="0.0" ixz="0.0" 
                      iyy="${i_value}" iyz="0.0" 
                      izz="${i_value}"/>
        </inertial>
    </xacro:macro>

    <xacro:include filename="$(find position_cont)/urdf/materials.xacro"/>
    <xacro:include filename="$(find position_cont)/urdf/robot.gazebo"/>

    <link name="world"/>
    <joint name="fixed" type="fixed">
        <parent link="world"/>
        <child link="base_link"/>
    </joint>

    <!-- Base Link -->
    <link name="base_link">
        <visual>
            <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
            <geometry>
                <box size="${width} ${width} ${height1}"/>
            </geometry>
            <material name="red"/>
        </visual>
        <collision>
            <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
            <geometry>
                <box size="${width} ${width} ${height1}"/>
            </geometry>
        </collision>

        <xacro:default_inertial z_value="${height1/2}" i_value="1.0" mass="1" />
    </link>
    <!-- Middle Link -->
    <link name="mid_link">
        <visual>
            <origin xyz="0 0 ${height2/2}" rpy="0 0 0"/>
            <geometry>
                <box size="${width} ${width} ${height2}"/>
            </geometry>
            <material name="green"/>
        </visual>
        <collision>
            <origin xyz="0 0 ${height2/2}" rpy="0 0 0"/>
            <geometry>
                <box size="${width} ${width} ${height2}"/>
            </geometry>
        </collision>
        <xacro:default_inertial z_value="${height2/2}" i_value="1.0" mass="1" />
    </link>

     <!-- Joint Between Base Link and Middle Link -->
     <joint name="joint1" type="revolute">
        <parent link="base_link"/>
        <child link="mid_link"/>
        <origin xyz="0 ${width} ${height1-axle_offset}" rpy="0 0 0"/>
        <axis xyz="0 1 0"/>
        <dynamics damping="${damp}"/>
        <limit effort="100.0" velocity="0.5" lower="-3.14" upper="3.14"/>
    </joint>

    <transmission name="transmission1">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="joint1">
          <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
        </joint>
        <actuator name="motor1">
          <hardwareInterface>EffortJointInterface</hardwareInterface>
          <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>


</robot>

robot.gazebo

<?xml version="1.0"?>
<robot>
    <!-- Base Link -->
    <gazebo reference="base_link">
        <material>Gazebo/Red</material>
    </gazebo>

    <!-- Middle Link -->
    <gazebo reference="mid_link">
        <mu1>0.2</mu1>
        <mu2>0.2< ...