History

Running a quadrotor simulation

This tutorial a set of ready-to-use sample files and scripts that should get you started for flying your first quadrotor in simulation. Is is not meant to be an exhaustive reference manual.

Table of Contents

Required components
Controlling via a TCL script
Controlling via a python script
Getting further

Required components

Before anything else, the following components should be installed on your system:

Software architecture
- rotorcraft-genom3: Low level hardware controller for multi-rotor robots talking the telekyb3 protocol.
- nhfc-genom3: A "near hovering" flight controller.
- pom-genom3: State estimation by fusion of multiple sensors (IMU, gps, motion capture …).
- optitrack-genom3: Driver for an Optitrack motion capture system.
Simulation software
- mrsim-gazebo: a gazebo plugin for multi-rotor simulation.
- optitrack-gazebo: a gazebo plugin for motion capture simulation.
Supervision software
- eltclsh: interactive TCL interpreter.
- genomix: HTTP server providing a generic interface between clients and genom components.
- tcl-genomix: TCL package for controlling genom components.

Refer to the software installation instructions for details. In particular, if you use robotpkg and the package sets described in the documentation, you can install the desired components by installing the package sets telekyb3, simulation and genom3.

Once everything is installed, you should be able to use the following tk3-quadrotor-simulation.sh shell script to start the required software. It is installed along with this documentation in $HOME/openrobots/share/doc/telekyb3/examples/bin/, or you can download it from here directly.

the script uses the pocolibs middleware and $HOME/openrobots prefix by default. Tune it if you changed the defaults.

Sample quadrotor simulation startup script

#!/bin/sh

# settings
middleware=pocolibs # or ros
components="
  rotorcraft
  nhfc
  pom
  optitrack
"
gzworld=$HOME/openrobots/share/gazebo/worlds/example.world


# list of process ids to clean, populated after each spawn
pids=

# cleanup, called after ctrl-C
atexit() {
    trap - 0 INT CHLD
    set +e

    kill $pids
    wait
    case $middleware in
        pocolibs) h2 end;;
    esac
    exit 0
}
trap atexit 0 INT
set -e

# init middleware
case $middleware in
    pocolibs) h2 init;;
    ros) roscore & pids="$pids $!";;
esac

# optionally run a genomix server for remote control
genomixd & pids="$pids $!"

# spawn required components
for c in $components; do
    $c-$middleware & pids="$pids $!"
done

# start gazebo
gzserver $gzworld & pids="$pids $!"
gzclient & pids="$pids $!"

# wait for ctrl-C or any background process failure
trap atexit CHLD
wait

To start all the required components, just run the script:

$ sh ./tk3-quadrotor-simulation.sh
Initializing pocolibs devices: OK
[...]
gzserver: mrsim created /tmp/pty-hr6
gzserver: mrsim created /tmp/pty-qr4

This should start all the components and finally a gazebo client showing two robots (one quadrotor and one hexarotor).

Figure 1. Simulation start

To stop the simulation, just hit ctrl-C in the terminal where the script is running.

Controlling via a TCL script

To control the running simulation, the following tk3-quadrotor-control.tcl TCL script can be used. It is installed along with this documentation in $HOME/openrobots/share/doc/telekyb3/examples/bin/, or you can download it from here directly.

Sample quadrotor TCL control script

#
package require genomix

# this connects to components running on the same host (localhost)
set g [genomix::localhost]
# to instead control components running on the remote computer "hostname" use
# set g [genomix::connect hostname]

# adapt path to your setup
$g rpath $env(HOME)/openrobots/lib/genom/pocolibs/plugins

# load components clients
$g load optitrack
$g load rotorcraft
$g load pom
$g load nhfc

# --- setup ----------------------------------------------------------------
#
# configure components, to be called interactively
proc setup {} {
  # optitrack
  #
  # connect to the simulated optitrack system on localhost
  ::optitrack::connect {
    host localhost host_port 1509 mcast "" mcast_port 0
  }


  # rotorcraft
  #
  # connect to the simulated quadrotor
  ::rotorcraft::connect {serial /tmp/pty-qr4 baud 0}

  # get IMU at 1kHz and motor data at 20Hz
  ::rotorcraft::set_sensor_rate {rate {imu 1000 mag 0 motor 20 battery 1}}

  # Filter IMU: 20Hz cut-off frequency for gyroscopes and 5Hz for
  # accelerometers. This is important for cancelling vibrations.
  ::rotorcraft::set_imu_filter {
    gfc {0 20 1 20 2 20}
    afc {0 5 1 5 2 5}
    mfc {0 20 1 20 2 20}
  }

  # read propellers velocities from nhfc controller
  ::rotorcraft::connect_port {
    local rotor_input remote nhfc/rotor_input
  }


  # nhfc
  #
  # configure quadrotor geometry: 4 rotors, not tilted, 23cm arms
  ::nhfc::set_gtmrp_geom {
    rotors 4 cx 0 cy 0 cz 0 armlen 0.23 mass 1.28
    rx 0 ry 0 rz -1 cf 6.5e-4 ct 1e-5
  }

  # emergency descent parameters
  ::nhfc::set_emerg {emerg {descent 0.1 dx 0.5 dq 1 dv 3 dw 3}}

  # PID tuning
  ::nhfc::set_saturation {sat {x 1 v 1 ix 0}}
  ::nhfc::set_servo_gain {
    gain {
      Kpxy 5 Kpz 5 Kqxy 4 Kqz 0.1 Kvxy 6 Kvz 6 Kwxy 1 Kwz 0.1
      Kixy 0. Kiz 0.
    }
  }

  # use tilt-prioritized controller
  ::nhfc::set_control_mode {att_mode ::nhfc::tilt_prioritized}

  # read measured propeller velocities from rotorcraft
  ::nhfc::connect_port {
    local rotor_measure remote rotorcraft/rotor_measure
  }

  # read current state from pom
  ::nhfc::connect_port {
    local state remote pom/frame/robot
  }


  # pom
  #
  # configure kalman filter
  pom::set_prediction_model ::pom::constant_acceleration
  pom::set_process_noise {max_jerk 100 max_dw 50}

  # allow sensor data up to 250ms old
  pom::set_history_length {history_length 0.25}

  # configure magnetic field
  pom::set_mag_field {
    magdir {
      x 23.8e-06
      y -0.4e-06
      z -39.8e-06
    }
  }

  # read IMU and magnetometers from rotorcraft
  ::pom::connect_port {local measure/imu remote rotorcraft/imu}
  ::pom::add_measurement imu 0 0 0 0 0 0
  ::pom::connect_port {local measure/mag remote rotorcraft/mag}
  ::pom::add_measurement mag 0 0 0 0 0 0

  # read position and orientation from optitrack
  ::pom::connect_port {local measure/mocap remote optitrack/bodies/QR_4}
  ::pom::add_measurement mocap 0 0 0 0 0 0
}


# --- start ----------------------------------------------------------------
#
# Spin the motors and servo on current position. To be called interactively
proc start {} {
  pom::log_state /tmp/pom.log 1
  pom::log_measurements /tmp/pom-measurements.log

  optitrack::set_logfile /tmp/opti.log

  rotorcraft::log /tmp/rotorcraft.log 1
  rotorcraft::start
  rotorcraft::servo &

  nhfc::log /tmp/nhfc.log 1
  nhfc::set_current_position
  nhfc::servo &
}


# --- stop -----------------------------------------------------------------
#
# Stop motors. To be called interactively
proc stop {} {
  rotorcraft::stop
  rotorcraft::log_stop

  nhfc::stop
  nhfc::log_stop

  pom::log_stop

  optitrack::unset_logfile
}

# start interactive loop
interactive

## interactively, one can start the simulation with
# setup
# start
## and then for instance set a desired position with
# nhfc::set_position 0 0 1 0
## to stop, use
# stop

To run the script, use the eltclsh tcl interpreter. This permis interactive control of the robot. After the prompt, issue the setup command for configuring the software and then the start command to start spinning the propellers and servo on the current position:

$ eltclsh ./tk3-quadrotor-control.tcl

                        eltclsh1.18 - Copyright (C) 2001-2020 LAAS-CNRS

eltclsh > setup
eltclsh > start
nhfc::0
eltclsh >

You should see the quadrotor propellers spinning and the robot hovering at the ground level.

You can then use the nhfc::set_position request to change the robot position. For instance:

eltclsh > ::nhfc::set_position {x 0 y 0 z 1 yaw 0}
eltclsh > ::nhfc::set_position {x 1 y 0 z 1 yaw 3.146}

The quadrotor should now be hovering abobe the hexarotor.

Controlling via a python script

To control the running simulation, the following tk3-quadrotor-control.py python script can be used. It is installed along with this documentation in $HOME/openrobots/share/doc/telekyb3/examples/bin/, or you can download it .from here directly.

Sample quadrotor python control script

#
import genomix
import os

# this connects to components running on the same host (localhost)
g = genomix.connect()
# to instead control components running on the remote computer "hostname" use
# g = genomix.connect('hostname')

# adapt path to your setup
g.rpath(os.environ['HOME'] + '/openrobots/lib/genom/pocolibs/plugins')

# load components clients
optitrack = g.load('optitrack')
rotorcraft = g.load('rotorcraft')
pom = g.load('pom')
nhfc = g.load('nhfc')

# --- setup ----------------------------------------------------------------
#
# configure components, to be called interactively
def setup():
  # optitrack
  #
  # connect to the simulated optitrack system on localhost
  optitrack.connect({
    'host': 'localhost', 'host_port': '1509', 'mcast': '', 'mcast_port': '0'
  })


  # rotorcraft
  #
  # connect to the simulated quadrotor
  rotorcraft.connect({'serial': '/tmp/pty-qr4', 'baud': 0})

  # get IMU at 1kHz and motor data at 20Hz
  rotorcraft.set_sensor_rate({'rate': {
    'imu': 1000, 'mag': 0, 'motor': 20, 'battery': 1
  }})

  # Filter IMU: 20Hz cut-off frequency for gyroscopes and 5Hz for
  # accelerometers. This is important for cancelling vibrations.
  rotorcraft.set_imu_filter({
    'gfc': [20, 20, 20], 'afc': [5, 5, 5], 'mfc': [20, 20, 20]
  })

  # read propellers velocities from nhfc controller
  rotorcraft.connect_port({
    'local': 'rotor_input', 'remote': 'nhfc/rotor_input'
  })


  # nhfc
  #
  # configure quadrotor geometry: 4 rotors, not tilted, 23cm arms
  nhfc.set_gtmrp_geom({
    'rotors': 4, 'cx': 0, 'cy': 0, 'cz': 0, 'armlen': 0.23, 'mass': 1.28,
    'rx':0, 'ry': 0, 'rz': -1, 'cf': 6.5e-4, 'ct': 1e-5
  })

  # emergency descent parameters
  nhfc.set_emerg({'emerg': {
    'descent': 0.1, 'dx': 0.5, 'dq': 1, 'dv': 3, 'dw': 3
  }})

  # PID tuning
  nhfc.set_saturation({'sat': {'x': 1, 'v': 1, 'ix': 0}})
  nhfc.set_servo_gain({ 'gain': {
    'Kpxy': 5, 'Kpz': 5, 'Kqxy': 4, 'Kqz': 0.1,
    'Kvxy': 6, 'Kvz': 6, 'Kwxy': 1, 'Kwz': 0.1,
    'Kixy': 0, 'Kiz': 0
  }})

  # use tilt-prioritized controller
  nhfc.set_control_mode({'att_mode': '::nhfc::tilt_prioritized'})

  # read measured propeller velocities from rotorcraft
  nhfc.connect_port({
    'local': 'rotor_measure', 'remote': 'rotorcraft/rotor_measure'
  })

  # read current state from pom
  nhfc.connect_port({
    'local': 'state', 'remote': 'pom/frame/robot'
  })


  # pom
  #
  # configure kalman filter
  pom.set_prediction_model('::pom::constant_acceleration')
  pom.set_process_noise({'max_jerk': 100, 'max_dw': 50})

  # allow sensor data up to 250ms old
  pom.set_history_length({'history_length': 0.25})

  # configure magnetic field
  pom.set_mag_field({'magdir': {
    'x': 23.8e-06, 'y': -0.4e-06, 'z': -39.8e-06
  }})

  # read IMU and magnetometers from rotorcraft
  pom.connect_port({'local': 'measure/imu', 'remote': 'rotorcraft/imu'})
  pom.add_measurement('imu')
  pom.connect_port({'local': 'measure/mag', 'remote': 'rotorcraft/mag'})
  pom.add_measurement('mag')

  # read position and orientation from optitrack
  pom.connect_port({
    'local': 'measure/mocap', 'remote': 'optitrack/bodies/QR_4'
  })
  pom.add_measurement('mocap')


# --- start ----------------------------------------------------------------
#
# Spin the motors and servo on current position. To be called interactively
def start():
  pom.log_state('/tmp/pom.log')
  pom.log_measurements('/tmp/pom-measurements.log')

  optitrack.set_logfile('/tmp/opti.log')

  rotorcraft.log('/tmp/rotorcraft.log')
  rotorcraft.start()
  rotorcraft.servo(ack=True) # this runs until stopped or input error

  nhfc.log('/tmp/nhfc.log')
  nhfc.set_current_position() # hover on current position


# --- stop -----------------------------------------------------------------
#
# Stop motors. To be called interactively
def stop():
  rotorcraft.stop()
  rotorcraft.log_stop()

  nhfc.stop()
  nhfc.log_stop()

  pom.log_stop()

  optitrack.unset_logfile()


## interactively, one can start the simulation with
# setup()
# start()
## and then for instance set a desired position with
# nhfc.set_position(0, 0, 1, 0)
## to stop, use
# stop

To run the script, use the python3 -i python interpreter. This permits interactive control of the robot. After the prompt, run the setup() procedure for configuring the software and then the start() procedure to start spinning the propellers and hover on the current position:

$ python3 -i ./tk3-quadrotor-control.py
>>> setup()
>>> start()
>>>

You should see the quadrotor propellers spinning and the robot hovering at the ground level.

You can then use the nhfc.set_position() service to change the robot position. For instance:

>>> nhfc.set_position()
double x > 0
double y > 0
double z > 1
double yaw > 0
>>> nhfc.set_position(1, 0, 1, 0)
>>> nhfc.set_position({'x': 1, 'y': 0, 'z': 1, 'yaw': 3.1416})

The quadrotor should now be hovering abobe the hexarotor.

Getting further

You can achive more complex simulation by updating the provided scripts and adding more components.

For instance, the maneuver-genom3 component can be used to plan feasible trajectories and join multiple waypoints.

Project

General

Profile

Aerial Robotics Testbed » telekyb3

Contents

Indexes

Running a quadrotor simulation

Required components

Controlling via a TCL script

Controlling via a python script

Getting further