Problem with two joints in arm

edit

Hi!

I'm trying to build an arm to put on a quadrotor. I've written a urdf file to describe this arm. When I only have one joint everything works well, I can control this joint and everything is correct, but when I enable the second arm joint start to shake and I can't control it. It happens when I change joint "shoulder_y_arm_joint" from "fixed" to "revolute":

 <link name="arm_base_link">
      <inertial>
        <mass value="0.00001" />
        <origin xyz="0.0625 0.065 0.203" />
        <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
      </inertial>

      <visual>
        <origin xyz="0.0625 0.065 0.203" rpy="0.0 0.0 -2.3561925" />
        <geometry>
          <mesh filename="package://uav/Media/models/bonebraker/brazo0.dae"/>
        </geometry>
      </visual>

      <collision>
        <origin xyz="0.0625 0.065 0.203" rpy="0.0 0.0 -2.3561925" />
        <geometry>
          <mesh filename="package://uav/Media/models/bonebraker/brazo0.dae"/>
        </geometry>
      </collision>
    </link>
 <joint name="base_arm_joint" type="fixed">
        <parent link="base_link"/>
    <child link="arm_base_link"/>
  </joint>

<link name="shoulder_0">
      <inertial>
        <mass value="0.01" />
        <origin xyz="0.0 0.0 0.0" />
        <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
      </inertial>

      <visual>
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -0.0" />
        <geometry>
          <mesh filename="package://uav/Media/models/bonebraker/brazo1.dae"/>
        </geometry>
      </visual>

      <collision>
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -0.0" />
        <geometry>
          <sphere radius='0.0002'/>
          <!--<mesh filename="package://uav/Media/models/bonebraker/brazo1.dae"/>-->
        </geometry>
      </collision>
    </link>
 <joint name="shoulder_y_arm_joint" type="fixed">
    <axis xyz="0 0 1"/>
        <limit effort="1000" lower="-0.8726" upper="0.8726" velocity="0.001"/>
        <origin xyz="0.048 0.050 0.155" rpy="0.0 0.0 -2.3561925" />
        <parent link="arm_base_link"/>
    <child link="shoulder_0"/>
  </joint>

<link name="shoulder_1">
      <inertial>
        <mass value="0.00001" />
        <origin xyz="0.0 0.0 0.0" />
        <inertia ixx="0.00007" ixy="0.0" ixz="0.0" iyy="0.00007" iyz="0.0" izz="0.00007" />
      </inertial>

      <visual>
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -0.0" />
        <geometry>
          <mesh filename="package://uav/Media/models/bonebraker/brazo2.dae"/>
        </geometry>
      </visual>

      <collision>
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -0.0" />
        <geometry>
           <sphere radius='0.0002'/>
        </geometry>
      </collision>
    </link>
 <joint name="shoulder_p_arm_joint" type="revolute">
    <axis xyz="0 1 0"/>
        <limit effort="0.1" lower="-3.14159" upper="1.0" velocity="0.001"/>
    <origin xyz="0.008 -0.00 -0.032" rpy="0.0 0.0 0.0" />
        <parent link="shoulder_0"/>
    <child link="shoulder_1"/>
  </joint>

Can you help me? Thank you in advance.