Robotics StackExchange | Archived questions

Gazebo_ros_control problem

Hello,

I am a beginner using ROS and gazebo, and my goal is to simulate a wheelchair. I have created a simple representation like this one :

image description

I now want to make the whair moving forward by adding a velocity, using the gazeboroscontrol plugin.

I followed this tutorial : https://dvic.devinci.fr/resource/tutorial/environnement-de-simulation-partie2/

I set the inertial values and the mass of each element : seat (+ one person) : 115kg front wheels : 250g back wheels : 1.5kg

I don't know how to set the collisions surfaces parameters (mu1 mu2 kp kd fdir1), so according to some research I put : mu1 = mu2 = 1.0 kp = 1e+8 kd = 1.0 fdir1 = '1 0 0'

I also set the launch and the config files, all the codes are below. However, when I start the simulation I got this :

image description

All the wheels are at 0 0 0 and under the seat. I don't know why. Finally, I don't know how to configure the pid gains in the config files .yaml, I guess this is the problem, with the fact that the masse is at around 120kg.

<?xml version='1.0' ?>


<link name='base_seat_link'>

        <visual>
            <origin xyz='0 0 0.4125' rpy='0 0 0'/>
            <geometry>
                <box size='0.675 0.675 0.675'/>
            </geometry>
            <material name='orange'>
                <color rgba="1 0.5 0 1" />
            </material>
        </visual>

    <collision>
        <origin xyz='0 0 0.4125' rpy='0 0 0'/>
            <geometry>
                <box size='0.675 0.675 0.675'/>
            </geometry>
            <material name='orange'>
                <color rgba="1 0.5 0 1" />
            </material>
    </collision>

        <inertial>
        <!-- masse : complete_seat=15kg + person=100kg -->
            <mass value='115.0'/>
        <!-- full cylinder r=0.5m et h=1.3m --> 
            <inertia ixx='23.3833' iyy='23.3833' izz='14.375' ixy='0' ixz='0' iyz='0'/>
        <origin xyz='0 0 0.4125' rpy='0 0 0' />
        </inertial>    

</link>

<link name='back_seat_link'>

        <visual>
            <origin xyz='0 0 0' rpy='0 0 0'/>
            <geometry>
                <box size='0.675 0.1 0.4'/>
            </geometry>
            <material name='orange'>
                <color rgba="1 0.5 0 1" />
            </material>
        </visual>

    <collision>
        <origin xyz='0 0 0' rpy='0 0 0'/>
            <geometry>
                <box size='0.675 0.1 0.4'/>
            </geometry>
            <material name='orange'>
                <color rgba="1 0.5 0 1" />
            </material>
    </collision>

        <inertial>
            <mass value='0.1'/>
            <inertia ixx='1e-6' iyy='1e-6' izz='1e-6' ixy='0' ixz='0' iyz='0'/>
        <origin xyz='0 -0.2875 0.9500' rpy='0 0 0' />
        </inertial>    

</link>

<joint name='seat_joint' type='fixed'>
        <axis xyz='1 0 0'/>
        <parent link ='base_seat_link'/>
        <child link ='back_seat_link'/>
        <origin xyz='0 -0.2875 0.9500' rpy='0 0 0' />
</joint>

<link name='front_left_wheel'>

    <visual>
        <origin xyz='0 0 0' rpy='0 1.57079 0' />
        <geometry>
                <cylinder radius='0.1' length='0.025'/>
            </geometry>
        <material name='white'>
            <color rgba='1 1 1 1' />
        </material>
    </visual>

    <collision>
        <origin xyz='0 0 0' rpy='0 1.57079 0' />
        <geometry>
                <cylinder radius='0.1' length='0.025'/>
            </geometry>
        <material name='white'>
            <color rgba='1 1 1 1' />
        </material>
        <surface>
            <mu1 value='1.0'/>
            <mu2 value='1.0'/>
            <kp value='1e+8' />
            <kd value='1.0' />
            <fdir1 value='1 0 0'/>
        </surface>
    </collision>

    <inertial>
        <!-- between 165g et 250g -->
                <mass value='0.250'/>
        <!-- empty cylinder R=10cm(8" min 3") r=7.5cm h=2.5cm (1") -->
                <inertia ixx='9.89e-4' iyy='9.89e-4' izz='1.95e-3' ixy='0' ixz='0' iyz='0'/>    
        <origin xyz='-0.35 0.2875 0.0975' rpy='0 1.57079 0' />
    </inertial>

</link>

<joint name='front_left_wheel_joint' type='continuous'>
    <!-- we consider the bottom of the wheelchair at 20cm from the ground -->
        <axis xyz='1 0 0'/>
        <parent link ='base_seat_link'/>
        <child link ='front_left_wheel'/>
        <origin xyz='-0.35 0.2875 0.0975' rpy='0 0 0' />
</joint>

<transmission name='front_left_wheel_trans' type='SimpleTransmission'>
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name='front_left_wheel_motor'>
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name='front_left_wheel_joint'>
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
</transmission>

<link name='front_right_wheel'>

    <visual>
        <origin xyz='0 0 0' rpy='0 1.57079 0' />
        <geometry>
                <cylinder radius='0.1' length='0.025'/>
            </geometry>
        <material name='white'>
            <color rgba='1 1 1 1' />
        </material>
    </visual>

    <collision>
        <origin xyz='0 0 0' rpy='0 1.57079 0' />
        <geometry>
                <cylinder radius='0.1' length='0.025'/>
            </geometry>
        <material name='white'>
            <color rgba='1 1 1 1' />
        </material>
        <surface>
            <mu1 value='1.0'/>
            <mu2 value='1.0'/>
            <kp value='1e+8' />
            <kd value='1.0' />
            <fdir1 value='1 0 0'/>
        </surface>
    </collision>

    <inertial> 
        <!-- entre 165g et 250g selon les roues -->
                <mass value='0.250'/>
        <!-- empty cylinder R=10cm (8" min 3") r=7.5 (1")-->
                <inertia ixx='9.89e-4' iyy='9.89e-4' izz='1.95e-3' ixy='0' ixz='0' iyz='0'/>    
        <origin xyz='0.35 0.2875 0.0975' rpy='0 1.57079 0' />
    </inertial>

</link>

<joint name='front_right_wheel_joint' type='continuous'>
    <!-- we consider the bottom of the wheelchair at 20cm from the ground -->
        <axis xyz='1 0 0'/>
        <parent link ='base_seat_link'/>
        <child link ='front_right_wheel'/>
        <origin xyz='0.35 0.2875 0.0975' rpy='0 0 0' />
</joint>

<transmission name='front_right_wheel_trans' type='SimpleTransmission'>
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name='front_right_wheel_motor'>
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name='front_right_wheel_joint'>
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
</transmission>

<link name='rear_left_wheel'>

    <visual>
        <origin xyz='0 0 0' rpy='0 1.57079 0' />
        <geometry>
                <cylinder radius='0.305' length='0.025'/>
            </geometry>
        <material name='white'>
            <color rgba='1 1 1 1' />
        </material>
    </visual>

    <collision>
        <origin xyz='0 0 0' rpy='0 1.57079 0' />
        <geometry>
                <cylinder radius='0.305' length='0.025'/>
            </geometry>
        <material name='white'>
            <color rgba='1 1 1 1' />
        </material>
        <surface>
            <mu1 value='1.0'/>
            <mu2 value='1.0'/>
            <kp value='1e+8' />
            <kd value='1.0' />
            <fdir1 value='1 0 0'/>
        </surface>
    </collision>

    <inertial>
        <!-- entre 1.15kg et 1.65kgg selon les roues -->
                <mass value='1.5'/>
        <!-- empty cylinder R=30.5cm (24") r=27cm (tire=3.5cm) -->
                <inertia ixx='6.23e-2' iyy='6.23e-2' izz='1.24e-1' ixy='0' ixz='0' iyz='0'/>
        <!-- for electrical seat : r0=18cm (14") r1=11.5cm-->
                <!--<inertia ixx='0' iyy='0.0342' izz='0.0342' ixy='0' ixz='0' iyz='0'/>-->
        <origin xyz='-0.35 -0.2875 0.305' rpy='0 1.57079 0' />
    </inertial>

</link>

<joint name='rear_left_wheel_joint' type='continuous'>
        <!-- we consider the bottom of the wheelchair at 20cm from the ground -->
        <axis xyz='1 0 0'/>
        <parent link ='base_seat_link'/>
        <child link ='rear_left_wheel'/>
        <origin xyz='-0.35 -0.2875 0.305' rpy='0 0 0' />
</joint>

<transmission name='rear_left_wheel_trans' type='SimpleTransmission'>
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name='rear_left_wheel_motor'>
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name='rear_left_wheel_joint'>
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
</transmission>

<link name='rear_right_wheel'>
    <visual>
        <origin xyz='0 0 0' rpy='0 1.57079 0' />
        <geometry>
                <cylinder radius='0.305' length='0.025'/>
            </geometry>
        <material name='white'>
            <color rgba='1 1 1 1' />
        </material>
    </visual>

    <collision>
        <origin xyz='0 0 0' rpy='0 1.57079 0' />
        <geometry>
                <cylinder radius='0.305' length='0.025'/>
            </geometry>
        <material name='white'>
            <color rgba='1 1 1 1' />
        </material>
        <surface>
            <mu1 value='1.0'/>
            <mu2 value='1.0'/>
            <kp value='1e+8' />
            <kd value='1.0' />
            <fdir1 value='1 0 0'/>
        </surface>
    </collision>

    <inertial> 
                <!--entre 1.15kg et 1.65kgg selon les roues-->
                <mass value='1.5'/>
        <!-- empty cylinder R=30.5cm (24") r=27cm (tire=6.5cm) -->
                <inertia ixx='6.23e-2' iyy='6.23e-2' izz='1.24e-1' ixy='0' ixz='0' iyz='0'/>
        <!-- for electrical seat : r0=18cm (14") r1=11.5cm-->
                <!--<inertia ixx='0' iyy='0.0342' izz='0.0342' ixy='0' ixz='0' iyz='0'/>-->
            <origin xyz='0 0 0' rpy='0 1.57079 0' />
    </inertial>

</link>

<joint name='rear_right_wheel_joint' type='continuous'>
        <!-- we consider the bottom of the wheelchair at 20cm from the ground -->
        <axis xyz='1 0 0'/>
        <parent link ='base_seat_link'/>
        <child link ='rear_right_wheel'/>
        <origin xyz='0.35 -0.2875 0.305' rpy='0 0 0' />
</joint>

<transmission name='rear_right_wheel_trans' type='SimpleTransmission'>
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name='rear_right_wheel_motor'>
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name='rear_right_wheel_joint'>
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
</transmission>

<gazebo>

    <plugin name='gazebo_ros_control' filename='libgazebo_ros_control.so'>
            <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
    </plugin>

</gazebo>

launch file :

<?xml version="1.0" ?>


    <!-- load the world file -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="paused" value="true"/>
        <arg name="use_sim_time" value="true"/>
</include>

<!-- load the robot description-->
<param command="$(find xacro)/xacro $(find my_robots)/robots_description/urdf/wheelchair_test.urdf" name="robot_description" />

<arg name="x" default="0"/>
<arg name="y" default="0"/>
<!--<arg name="z" default="0.4125"/>-->
<arg name="z" default="0"/>

<!-- load the node which launch gazebo with the robot description-->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"  output="screen" 
    args="-urdf -param robot_description  -model wheelchair_test -x $(arg x) -y $(arg y) -z $(arg z)" />

<!-- load controller settings -->
<rosparam command="load" file="$(find my_robots)/config/wheelchair_test_control.yaml" />

<!-- load the controller manager -->
<node name="controller_manager" pkg="controller_manager" type="spawner" output="screen" args="joint_publisher vel_controller"/>

config file :

jointpublisher: type: "jointstatecontroller/JointStateController" publishrate: 50

velcontroller: type: "diffdrivecontroller/DiffDriveController" leftwheel: 'rearleftwheeljoint' rightwheel: 'rearrightwheeljoint' publishrate: 50

posecovariancediagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03] twistcovariancediagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]

# velocity command timeout [s], default 0.5 cmdveltimeout: 0.5

# wheel separation and diameter. These are both optional. # diffdrivecontroller will attempt to read either one or both from the # URDF if not specified as a parameter # wheelseparation : 0.7 # wheelradius : 0.61

baseframeid: baseseatlink # default baselink odomframeid: odom enableodom_tf: true

estimatevelocityfrom_position: false

# wheel separation and radius multipliers wheelseparationmultiplier: 1.0 # default: 1.0 wheelradiusmultiplier : 1.0 # default: 1.0

# velocity and acceleration limits linear: x: hasvelocitylimits : true maxvelocity : 3.0
hasaccelerationlimits: true maxacceleration : 2.0
angular: z: hasvelocitylimits : true maxvelocity : 3.0
hasaccelerationlimits: true maxacceleration : 2.0

/gazeboroscontrol: pidgains: rearleftwheeljoint: {p: 200, i: 0.01, d: 10.0, iclamp: 0.0} rearrightwheeljoint: {p: 200, i: 0.01, d: 10.0, i_clamp: 0.0}

I use gazebo9, ros-melodic on ubuntu 18.04 Thank you very much for any help.

Asked by doriiber on 2020-06-05 11:38:44 UTC

Comments

Hi @doriiber,

First of all I recommend you Xacro, it will save you time, effort and a lot of useless code.

Then, usually with newer version of Gazebo you can run your simulation with deafult collision and friction values to study the behaviour of your model and then compute your own values based on experimentation.

Finally, I see that you are using a VelocityJointInterface for wheel control, I recommend you to adjust the PID values because it seems that the gains are too high for your simualtion. Just start with a P=1 and then increase the values to reach your desired stability. Furthemore, pay attention to the derivative gain, my experienced is that higher values increase exponentially the inestability of the simulation.

Hope it helps!

Asked by Weasfas on 2020-06-06 05:45:11 UTC

Thank you very much for your response ! It solved my problem. I also have rebuild my file with xacro, it is quite better. For people who may have the same issue, take into account that you may have the same issue if there is no inertial values on your controlled wheels.

Thanks a lot !

Asked by doriiber on 2020-06-07 11:18:33 UTC

Great you solved the problem!. If you do not mind, can you post your solution as an answer aand accept it?

Regards.

Asked by Weasfas on 2020-06-08 09:46:46 UTC

Answers

Thank you very much Weasfas for your help !

Hi @doriiber,

First of all I recommend you Xacro, it will save you time, effort and a lot of useless code.

Then, usually with newer version of Gazebo you can run your simulation with deafult collision and friction values to study the behaviour of your model and then compute your own values based on experimentation.

Finally, I see that you are using a VelocityJointInterface for wheel control, I recommend you to adjust the PID values because it seems that the gains are too high for your simualtion. Just start with a P=1 and then increase the values to reach your desired stability. Furthemore, pay attention to the derivative gain, my experienced is that higher values increase exponentially the inestability of the simulation.

Hope it helps! Weasfas gravatar image Weasfas ( Jun 6 '0 )

Asked by doriiber on 2020-06-10 11:21:23 UTC

Comments