Guided and precise motion of the drone#

This tutorial shows how to use handling components to create scripted animations. They are mostly used to simulate actions that would otherwise require a human operator.

Each drone may come with one or several instances of the handling component. For example, ANAFI Ai comes with a component to simulate a user launching the drone into the air, but also another one to simulate a user calibrating the drone.

Example: ANAFI Ai user calibration

Component parameters can be manipulated in the Web dashboard. See Interact with the simulation for more information on how to interact with components. For the complete list of handling actions and parameters, see Handling.

How it works#

Several proportional-integral-derivative (PID) are used to control the location and orientation of the drone. The animation is provided as a script written in JSON format. It contains a series of steps executed successively, where each step contains two sets of ExprTk expressions used to animate the drone and a stop condition to move to the next step.

See How to write expression to change HW component behavior for the list of variables provided by Parrot Sphinx in the expression evaluation context. Here are additional variables provided by handling components only:

  • val: current value for corresponding drone coordinate

  • start_val: starting value for corresponding drone coordinate determined by the first_pose object in the same step

  • script_time: elapsed time since the beginning of the script [s]

  • step_time: elapsed time since the beginning of the step [s]

  • motor_speed_0: front-left motor speed [m/s]

  • motor_speed_1: front-right motor speed [m/s]

  • motor_speed_2: rear-left motor speed [m/s]

  • motor_speed_3: rear-right motor speed [m/s]

  • start_posX: initial X coordinate in world frame [m]

  • start_posY: initial Y coordinate in world frame [m]

  • start_posZ: initial Z coordinate in world frame [m]

  • start_speedX: initial linear body speeds [m/s]

  • start_speedY: initial linear body speeds [m/s]

  • start_speedZ: initial linear body speeds [m/s]

  • start_accX: initial linear body accelerations [m/s2]

  • start_accY: initial linear body accelerations [m/s2]

  • start_accZ: initial linear body accelerations [m/s2]

  • start_p: initial roll [rad]

  • start_q: initial pitch [rad]

  • start_r: initial yaw [rad]

  • start_phi: initial angular body speeds [rad/s]

  • start_theta: initial angular body speeds [rad/s]

  • start_psi: initial angular body speeds [rad/s]

  • start_phidot: initial X angular body accelerations [rad/s2]

  • start_thetadot: initial Y angular body accelerations [rad/s2]

  • start_psidot: initial Z angular body accelerations [rad/s2]

  • start_temperature: initial atmospheric pressure at the starting location of the drone. [°C]

  • start_pressure: initial atmospheric pressure at the starting location of the drone. [Pa]

A script can be started, stopped and paused, by sending the start, stop, and pause actions to the corresponding handling component.

During the execution of a script, the component parameter current_step_index is set to the index of the current step. It can be queried to monitor the progress of the script.

Let’s take a look at a simple handling script example which comes with ANAFI Ai to simulate a hand launch:

Example#

[
  {
    "stop_condition_expr": "posZ - start_posZ > 2",
    "first_pose": {
      "x_expr": "val",
      "y_expr": "val",
      "z_expr": "val",
      "roll_expr": "0",
      "pitch_expr": "0",
      "yaw_expr": "val",
      "relative_position": "false",
      "reset_all_forces": "true",
      "min_collision_radius": 0,
      "max_height_above_ground": 0
    },
    "step_pose": {
      "x_expr": "start_val",
      "y_expr": "start_val",
      "z_expr": "start_val + 2 * step_time",
      "roll_expr": "start_val + 0.05 * sin(2 * pi * step_time)",
      "pitch_expr": "start_val + 0.05 * sin(2 * pi * step_time)",
      "yaw_expr": "start_val + 0.05 * sin(2 * pi * step_time)"
    }
  },
  {
    "stop_condition_expr": "abs(motor_speed_0) > 300 or step_time > 40",
    "step_pose": {
      "x_expr": "start_val",
      "y_expr": "start_val",
      "z_expr": "start_val",
      "roll_expr": "start_val + 0.01 * sin(2 * pi * step_time)",
      "pitch_expr": "start_val + 0.01 * sin(2 * pi * step_time)",
      "yaw_expr": "start_val + 0.01 * sin(2 * pi * step_time)"
    }
  },
  {
    "stop_condition_expr": "step_time > 2",
    "step_pose": {
      "x_expr": "start_val",
      "y_expr": "start_val",
      "z_expr": "start_val",
      "roll_expr": "start_val + 0.01 * sin(2 * pi * step_time)",
      "pitch_expr": "start_val + 0.01 * sin(2 * pi * step_time)",
      "yaw_expr": "start_val + 0.01 * sin(2 * pi * step_time)"
    }
  },
  {
    "stop_condition_expr": "step_time > 0.1",
    "step_pose": {
      "x_expr": "start_val",
      "y_expr": "start_val",
      "z_expr": "start_val + 2 * step_time",
      "roll_expr": "start_val + 0.01 * sin(2 * pi * step_time)",
      "pitch_expr": "start_val + 0.01 * sin(2 * pi * step_time)",
      "yaw_expr": "start_val + 0.01 * sin(2 * pi * step_time)"
    }
  }
]

The code explained#

Let’s go through it by parts.

We start by defining a first step:

[
  {
    "stop_condition_expr": "posZ - start_posZ > 2",

The optional stop_condition_expr object is an ExprTk expression which determines when the step will end. In this example, we check if the Z coordinate of the drone posZ is more than 2 meters above the initial Z coordinate start_posZ. When this condition is met, the component moves on to the next step.

"first_pose": {
  "x_expr": "val",
  "y_expr": "val",
  "z_expr": "val",
  "roll_expr": "0",
  "pitch_expr": "0",
  "yaw_expr": "val",
  "relative_position": "false",
  "reset_all_forces": "true",
  "min_collision_radius": 0,
  "max_height_above_ground": 0
},

The optional first_pose object defines the starting pose of the drone. Each coordinate is described as an ExprTk expression. The x_expr, y_expr, and z_expr expressions are used to translate the drone and are expressed in ENU. In this example, the drone will start at its original position. The roll_expr, pitch_expr, and yaw_expr expressions are used to set the world orientation of the drone and are relative to the world frame. In this example, the drone will maintain its original yaw orientation and have its pitch and roll set to zero.

The optional relative_position parameter is used to specify whether the position is relative to the start position and orientation. By default, this parameter is set to false meaning that the position is relative to the world reference frame.

The optional reset_all_forces parameter is used to specify whether forces, accelerations and speeds currently applied on the drone must be zeroed before starting the step. By default, this parameter is set to true.

The optional min_collision_radius parameter specifies the radius of a sphere positioned at the center of the drone. If this value is greater than zero, a collision test is performed with the sphere shape. If a collision is detected with a surrounding object, the script is stopped.

The optional max_height_above_ground parameter specifies the maximum height above the ground that the drone can be at. If this value is greater than zero and the condition is not met, the script is stopped.

When the execution of a script is stopped or after completing successfully all its steps, the component parameter current_step_index is set to the number of steps in the script. Once this state is reached, you can query the component parameter invalid_step_index to see if the execution of the script was canceled due to a failed test. A value of -1 indicates that the execution was successful, otherwise the value corresponds to the index of the step that failed a test.

  "step_pose": {
    "x_expr": "start_val",
    "y_expr": "start_val",
    "z_expr": "start_val + 2 * step_time",
    "roll_expr": "start_val + 0.05 * sin(2 * pi * step_time)",
    "pitch_expr": "start_val + 0.05 * sin(2 * pi * step_time)",
    "yaw_expr": "start_val + 0.05 * sin(2 * pi * step_time)"
  }
},

The optional step_pose object contains expressions given in ENU that are applied to modify the pose of the drone during each world iteration after the drone has been positioned at its starting pose. Note that for each expression, the val variable is set to the value of corresponding drone coordinate, while the start_val variable is set to the corresponding “step” starting coordinate determined with first_pose. In this example, during each world iteration, a new target height is obtained from the elapsed step time and is used by the corresponding PID to generate a force which results in the drone being moved upwards. During the execution, the drone orientation is slightly disturbed by a sine wave with 1s period to simulate hand movements.

We now define a second step which starts immediately after the first one meets its stop condition:

{
  "stop_condition_expr": "abs(motor_speed_0) > 300 or step_time > 40",
  "step_pose": {
    "x_expr": "start_val",
    "y_expr": "start_val",
    "z_expr": "start_val",
    "roll_expr": "start_val + 0.01 * sin(2 * pi * step_time)",
    "pitch_expr": "start_val + 0.01 * sin(2 * pi * step_time)",
    "yaw_expr": "start_val + 0.01 * sin(2 * pi * step_time)"
  }
},

For this next step, the stop_condition_expr checks for the variable motor_speed_0 to detect that the motors have started and sets a timeout of 40 seconds to prevent the script from running indefinitely.

The third step simply continues shaking the drone for 2 seconds. After that time, the fourth step starts immediately:

{
  "stop_condition_expr": "step_time > 2",
  "step_pose": {
    "x_expr": "start_val",
    "y_expr": "start_val",
    "z_expr": "start_val",
    "roll_expr": "start_val + 0.01 * sin(2 * pi * step_time)",
    "pitch_expr": "start_val + 0.01 * sin(2 * pi * step_time)",
    "yaw_expr": "start_val + 0.01 * sin(2 * pi * step_time)"
  }
},

The fourth step is used to launch the drone into the air in 1/10th of a second. The first_pose expressions are defined to keep the same coordinates as the ones obtained at the end of the first step.

  {
    "stop_condition_expr": "step_time > 0.1",
    "step_pose": {
      "x_expr": "start_val",
      "y_expr": "start_val",
      "z_expr": "start_val + 2 * step_time",
      "roll_expr": "start_val + 0.01 * sin(2 * pi * step_time)",
      "pitch_expr": "start_val + 0.01 * sin(2 * pi * step_time)",
      "yaw_expr": "start_val + 0.01 * sin(2 * pi * step_time)"
    }
  }
]

During each world iteration, a new target height is obtained from the elapsed step time and is used by the corresponding PID to generate a force which results in the drone being thrown into the air.

Code snippets#

Below are code snippets that show how to create and start a scripted animation using either the sphinx-cli command line tool or pysphinx for Python users. In this example, the drone is spawned two meters above the world origin for 10 seconds.

Using sphinx-cli

script=$(cat << EOT
[
  {
    "stop_condition_expr": "step_time > 10",
    "first_pose": {
      "x_expr": "0",
      "y_expr": "0",
      "z_expr": "2",
      "roll_expr": "0",
      "pitch_expr": "0",
      "yaw_expr": "0"
    }
  }
]
EOT
)

sphinx-cli param handling/userscript script "${script}"
sphinx-cli action handling/userscript start

Using pysphinx

import pysphinx

sphinx = pysphinx.Sphinx()

handling_script = """
[
  {
    "stop_condition_expr": "step_time > 10",
    "first_pose": {
      "x_expr": "0",
      "y_expr": "0",
      "z_expr": "2",
      "roll_expr": "0",
      "pitch_expr": "0",
      "yaw_expr": "0"
    }
  }
]
"""

handling = sphinx.get_component("anafi_ai", "userscript", "handling")
handling.set_param("script", handling_script)
handling.trig_action("start")