Simulation for Inverted Pendulum on Quadrotor

fasdf · February 19, 2024, 4:32pm

I’m trying to implement an inverted pendulum on a custom quadrotor in Gazebo-Classic. This is my first time using Gazebo.

The quadrotor is done with the PX4 flight stack and I verified that it flies as intended in Gazebo. Now, I’m trying to put an inverted pendulum on top of the quadrotor and have the quadrotor to balance the pendulum. For the mounting method, I’m just going to use an empty cup. In other words, there will be no actual “joint” for mounting the pendulum on the drone. The pendulum will be inserted into this small empty cup since that’s how I will simulate in the real world.

I would appreciate help in the following parts:

How to put the pendulum on top of the quadrotor as a different entity. Again, there is no physical link between the two entities.
How to “start” the pendulum dynamics. Once the quadrotor takes off and has the custom controller started, I would like the pendulum to “appear” on the quadrotor in vertical position. Then the quadrotor controller will try to balance it. This is how I will run hardware experiments: have the quadrotor in stable hovering, start custom controller, and put the pendulum in the mount.

Thank you very much!

Benja · March 3, 2024, 9:42pm

Hi @fasdf , as this is your first time using Gazebo, you should consider removing the cup and link the pendulum to the drone with a universal joint.
Using the cup would require a lot of work on the collision geometry of the cup and the pendulum to allow one to sit on the other.

To start the simulation you could also add a fixed joint between drone and pendulum and programmatically disable it when you want to free the pendulum.

fasdf · March 13, 2024, 5:15am

Hi Benja, thank you for your response.
I am following your recommendation and am working with a universal joint. I am running into problems, however. I have a custom drone model named “danausA” and it runs well when I build it using PX4 SITL: make px4_sitl gazebo-classic_danausA. I added a universal joint for this pendulum in the SDF file for danausA to see how it’s rendered. I named this custom model “danausA_pend”. Now, when I run make px4_sitl gazebo-classic_danausA_pend, Gazebo does not give me anything! It doesn’t give me an error message nor renders the drone at all.

Here’s the universal joint part in the SDF file:

    <joint name='pendulum_joint' type='universal'>
      <pose relative_to='base_link'>0 0 0.12 0 0 0</pose>
      <parent>base_link</parent>
      <child>pendulum</child>
      <axis1>
        <xyz>1 0 0</xyz>
        <limit>
          <lower>-3.14159</lower>
          <upper>3.14159</upper>
        </limit>
        <dynamics>
          <spring_stiffness>0.0</spring_stiffness>
          <damping>0.0</damping>
        </dynamics>
      </axis1>
      <axis2>
        <xyz>0 1 0</xyz>
        <limit>
          <lower>-3.14159</lower>
          <upper>3.14159</upper>
        </limit>
        <dynamics>
          <spring_stiffness>0.0</spring_stiffness>
          <damping>0.0</damping>
        </dynamics>
      </axis2>
      <pose>0 0 0 0 0 0</pose>
    </joint>
    <link name='pendulum'>
      <pose relative_to='pendulum_joint'>0 0 0 0 0 0</pose>
      <inertial>
        <pose>0 0 0 0 0 0</pose>
        <mass>0.03133884</mass>
        <inertia>
          <ixx>0.00261178</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.00261178</iyy>
          <iyz>0</iyz>
          <izz>0.00000042</izz>
      </inertial>
      <collision name='pendulum_collision'>
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <cylinder>
            <length>1</length>
            <radius>0.004064</radius>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='pendulum_visual'>
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <cylinder>
            <length>1</length>
            <radius>0.004064</radius>
          </cylinder>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Red</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay/>
    </link>

Here’s the log:

ubuntu@2222:~/PX4-Autopilot(main)$ make px4_sitl gazebo-classic_danausA_pend
[0/4] Performing build step for 'sitl_gazebo-classic'
ninja: no work to do.
[3/4] cd /home/ubuntu/PX4-Auto...-Autopilot/build/px4_sitl_default
SITL ARGS
sitl_bin: /home/ubuntu/PX4-Autopilot/build/px4_sitl_default/bin/px4
debugger: none
model: danausA_pend
world: none
src_path: /home/ubuntu/PX4-Autopilot
build_path: /home/ubuntu/PX4-Autopilot/build/px4_sitl_default
GAZEBO_PLUGIN_PATH :/home/ubuntu/PX4-Autopilot/build/px4_sitl_default/build_gazebo-classic
GAZEBO_MODEL_PATH :/home/ubuntu/PX4-Autopilot/Tools/simulation/gazebo-classic/sitl_gazebo-classic/models
LD_LIBRARY_PATH /home/ubuntu/danaus_ros_ws/offboard_ctrl/devel/lib:/opt/ros/noetic/lib:/home/ubuntu/PX4-Autopilot/build/px4_sitl_default/build_gazebo-classic
empty world, setting empty.world as default
Using: /home/ubuntu/PX4-Autopilot/Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/danausA_pend/danausA_pend.sdf
SITL COMMAND: "/home/ubuntu/PX4-Autopilot/build/px4_sitl_default/bin/px4" "/home/ubuntu/PX4-Autopilot/build/px4_sitl_default"/etc

______  __   __    ___ 
| ___ \ \ \ / /   /   |
| |_/ /  \ V /   / /| |
|  __/   /   \  / /_| |
| |     / /^\ \ \___  |
\_|     \/   \/     |_/

px4 starting.

INFO  [px4] startup script: /bin/sh etc/init.d-posix/rcS 0
INFO  [init] found model autostart file as SYS_AUTOSTART=7011
INFO  [param] selected parameter default file parameters.bson
INFO  [param] importing from 'parameters.bson'
INFO  [parameters] BSON document size 316 bytes, decoded 316 bytes (INT32:13, FLOAT:3)
INFO  [param] selected parameter backup file parameters_backup.bson
  SYS_AUTOCONFIG: curr: 0 -> new: 1
+ SYS_AUTOSTART: curr: 7010 -> new: 7011
  SENS_BOARD_X_OFF: curr: 0.0000 -> new: 0.0000
  SENS_DPRES_OFF: curr: 0.0000 -> new: 0.0010
INFO  [dataman] data manager file './dataman' size is 7872608 bytes
INFO  [init] PX4_SIM_HOSTNAME: localhost
INFO  [simulator_mavlink] Waiting for simulator to accept connection on TCP port 4560
Gazebo multi-robot simulator, version 11.14.0
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 172.26.187.151

Gazebo shows me nothing and the log gets stuck here.

I am wondering if PX4 SITL even allows joints that are not rotors or some kind of actuators that can be directly controlled.

Benja · March 13, 2024, 8:36am

Hi @fasdf , could you share a link to the branch where you pushed these changes please?

fasdf · March 13, 2024, 7:04pm

Hi @Benja, thank you for the quick response. I have not pushed my code yet and will do so shortly this week.
As for the universal joints, it is working well now - I simply missed a </inertia> line.

I am now trying to dynamically disable the fixed joint as you recommended. If you have any suggestions, please let me know. I am currently looking into DetachableJoint as suggested by this post, but am not sure if this is a supported feature in Gazebo Classic.