SetJointPosition is not working properly (angles don't stay steady) [closed]

edit

Hi all,

I am using the SetJointPosition function in a simple Gazebo plugin to get my robot into a solid start position. The model is manually loaded in the default gazebo world.

At a first glance everything looked good, but then one hinge (hinge joint 2 (hinge_joint_1_hinge_joint_2_joint)) slips downwards and the robot started to slowly move forward. At some time the attended visuals of hinge_joint_2_link (connector visuals) fall apart while the following joints and links stay in position. Shortly after that the falling visual seem to hit the robot base and the complete robot flies through the air.

I have no idea what I am doing wrong. I think I use the function as explained in Basic Joint Control Question.

Here are my sdf models and the plugin code.

Robot Base:

<?xml version="1.0" ?>
<sdf version="1.3">
    <!-- Model Name = Namespace -->
    <model name="scitos">
        <static>false</static>
        <!-- Base Link-->
        <link name="base_link">
            <pose>0 0 0.34 0 0 0</pose>
            <visual name="base_visual">
                <geometry>
                    <cylinder>
                        <radius>0.30</radius>
                        <length>0.58</length>
                    </cylinder>
                </geometry>
                <material>
                    <ambient>0 0 0 1</ambient>
                </material>
            </visual>
            <collision name="base_collision">
                <geometry>
                    <cylinder>
                        <radius>0.30</radius>
                        <length>0.58</length>
                    </cylinder>
                </geometry>
            </collision>
            <!-- Tail Link und Laufrolle als Visuals-->
            <visual name="tail_visual">
                <pose>-0.30 0 -0.19 0 0 0</pose>
                <geometry>
                    <cylinder>
                        <radius>0.15</radius>
                        <length>0.20</length>
                    </cylinder>
                </geometry>
                <material>
                    <ambient>0 0 0 1</ambient>
                </material>
            </visual>
            <collision name="tail_collision">
                <pose>-0.30 0 -0.19 0 0 0</pose>
                <geometry>
                    <cylinder>
                        <radius>0.15</radius>
                        <length>0.20</length>
                    </cylinder>
                </geometry>
            </collision>
            <!-- Caster Wheel -->
            <visual name="caster_visual">
                <pose>-0.275 0 -0.29 0 0 0</pose>
                <geometry>
                    <sphere>
                        <radius>0.05</radius>
                    </sphere>
                </geometry>
            </visual>
            <collision name="caster_collision">
                <pose>-0.275 0 -0.29 0 0 0</pose>
                <geometry>
                    <sphere>
                        <radius>0.05</radius>
                    </sphere>
                </geometry>
                <surface>
                    <friction>
                        <ode>
                            <mu>0</mu>
                            <mu2>0</mu2>
                            <slip1>1.0</slip1>
                            <slip2>1.0</slip2>
                        </ode>
                    </friction>
                </surface>
            </collision>            
        </link>
        <!-- Left Wheel Link -->
        <link name="left_wheel_link">
            <pose>0.075 0.155 0.05 -1.57 0 0</pose>
            <collision name="left_wheel_collision">
                <pose>0 0 0 0 0 0</pose>
                <geometry>
                    <cylinder>
                        <radius>0.05</radius>
                        <length>0.025</length>
                    </cylinder>
                </geometry>
            </collision>
            <visual name="left_wheel_visual">
                <pose>0 0 0 0 0 0</pose>
                <geometry>
                    <cylinder>
                        <radius>0.05</radius>
                        <length>0.025</length>
                    </cylinder>
                </geometry>
            </visual>
        </link>
        <!-- Right Wheel Link -->
        <link name="right_wheel_link">
            <pose>0.075 -0.155 0.05 -1.57 0 0</pose>
            <collision name="right_wheel_collision">
                <pose>0 0 0 0 0 0</pose>
                <geometry>
                    <cylinder>
                        <radius>0.05</radius>
                        <length>0.025</length>
                    </cylinder>
                </geometry>
            </collision>
            <visual name="right_wheel_visual">
                <pose>0 0 0 0 0 0</pose>
                <geometry>
                    <cylinder>
                        <radius>0.05</radius>
                        <length>0.025</length>
                    </cylinder>
                </geometry>
            </visual>
        </link>
        <!-- Base Left Wheel Joint-->
        <joint name="base_left_wheel_joint" type="revolute">
            <pose>0 0 0 0 0 0</pose>
            <parent>base_link</parent>
            <child>left_wheel_link</child>
            <axis>
                <xyz>0 1 0</xyz>
            </axis>
        </joint ...