Robotic Manipulator with spherical joints

edit

Hello,

I am trying to make a robotic manipulator which has 3 links with a spherical joint on the first and last link and a revolute joint in the middle, mimicking a human arm. The issue I am facing is when I want to use inverse kinematics to control the arm, using the joint_group_position_controller, the model breaks in gazebo immediately. Where as if I use inverse dynamics and joint_group_effort_controller, it does appear and swings around, but does not reach the target position I give.

I made one with just 2 links with spherical joints on each and it worked fine, but when I add one link and joint, it either doesn't work or fails completely.

My urdf file for the robot arm is

<?xml version="1.0"?>
<robot name="pendulum_robot"
    xmlns:xacro="http://www.ros.org/wiki/xacro">

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

    <xacro:include filename="$(find robot)/urdf/materials.xacro" />
    <!-- <robot name="pendulum_robot"> -->
    <link name="world">
    </link>

    <material name="blue">
        <color rgba="1.0 0.8 0.8 1"/>
    </material>

    <material name="red">
        <color rgba="0.8 0 0 1"/>
    </material>

    <material name="yellow">
        <color rgba="0 0.5 0.5 1"/>
    </material>


    <joint name="base_joint" type="fixed">
        <parent link="world"/>
        <child link="body_link"/>
        <axis xyz="0.0 0.0 0.0"/>
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
    </joint>


    <!-- First Link -->
    <link name="body_link">
        <visual>
            <origin xyz="0 0 0.75" rpy="0 0 0"/>
            <geometry>
                <box size="0.1 0.1 1.5"/>
            </geometry>
        </visual>

        <!-- <collision>
            <origin xyz="0 0 0.75" rpy="0 0 0"/>
            <geometry>
                <box size="0.1 0.1 1.0"/>
            </geometry>
        </collision> -->

        <inertial>
            <origin xyz="0 0 0.75" rpy="0 0 0"/>
            <mass value="1"/>
            <inertia ixx="0.18833" ixy="0.0" ixz="0.0" iyy="0.18833" iyz="0.0" izz="0.001666667"/>
        </inertial>
    </link>

    <!-- Shoulder Joint Joint -->
    <joint name="shoulder_y" type="continuous">
        <parent link="body_link"/>
        <child link="dummy_link_1"/>
        <axis xyz="0 1 0" />
        <origin xyz="0.0 0 1.55" rpy="0 0 0"/>
        <!-- <limit effort="50" velocity="2.0"/> -->
        <dynamics damping="0.2"/>
    </joint>

    <link name="dummy_link_1">

        <inertial>
            <mass value="0.025" />
            <inertia ixx="5.8083e-4" ixy="0" ixz="0" iyy="3.0833e-5" iyz="0" izz="5.9083e-4" />
        </inertial>
    </link>

    <joint name="shoulder_z" type="continuous">
        <parent link="dummy_link_1"/>
        <child link="dummy_link_2"/>
        <axis xyz="0 0 1" />
        <dynamics damping="0.2"/>
        <!-- <limit effort="50" velocity="2.0"/> -->
    </joint>

    <link name="dummy_link_2">
        <inertial>
            <mass value="0.025" />
            <inertia ixx="5.8083e-4" ixy="0" ixz="0" iyy="3 ...