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 coordinatestart_val
: starting value for corresponding drone coordinate determined by thefirst_pose
object in the same stepscript_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")