Motion Capture Causing Crashes (Optitrack)

[mwlock]

Hey all, I am a realtive newbie to the world of Crazyflie drones and must say that the level of support here has been incredible ! A big thank you to the team for actively supporting the users 🙏

Now that I am ready for testing using a motion capture system, I am unfortunately facing some issues controlling the drone... Looking at the most simple use case; controlling the Crazyflie works perfectly well using the takeoff and land services when only using the flowdeck outside of the motion capture environment. But then when I move into the motion capture room, the drone becomes unstable very quickly and crashes...

For extra context, I am using the Optitrack system with Motive 2.3. My config files are shown below.

crazyflies.yaml

robots:
  cf1:
    enabled: true
    uri: radio://0/80/2M/E7E7E7E7E7
    initial_position: [0, 0, 0]
    type: cf21  # see robot_types

robot_types:
  cf21:
    motion_capture:
      enabled: false
      # only if enabled; see motion_capture.yaml
      marker: mocap_deck
      dynamics: default
    big_quad: false
    battery:
      voltage_warning: 3.8  # V
      voltage_critical: 3.7 # V

# global settings for all robots
all:
  firmware_logging:
    enabled: true
    default_topics:
     # remove to disable default topic
      pose:
        frequency: 80 # Hz
    custom_topics:
     velocity:
       frequency: 80 # Hz
       vars: ["stateEstimate.vx","stateEstimate.vy","stateEstimate.vz"] # Global reference frame
    
  firmware_params:
    commander:
      enHighLevel: 1
    stabilizer:
      estimator: 2 # 1: complementary, 2: kalman
      controller: 1 # 1: PID, 2: mellinger
    locSrv:
      extPosStdDev: 1e-3
      extQuatStdDev: 0.5e-1
  broadcasts:
    num_repeats: 15 # number of times broadcast commands are repeated
    delay_between_repeats_ms: 1 # delay in milliseconds between individual repeats

motion_capture.yaml

/motion_capture_tracking:
  ros__parameters:
    type: "optitrack_closed_source"
    hostname: "192.168.0.101"

    mode: "motionCapture" # one of motionCapture,libRigidBodyTracker

    topics:
      poses:
        qos:
          mode: "sensor"
          deadline: 100.0 # Hz

    marker_configurations:
      default: # for standard Crazyflie
        offset: [0.0, 0.0, 0.0]
        points:
          p0: [   0.0,    0.0, 0.022]  # top
          p1: [-0.042,  0.042, 0.0  ]  # back left   (M3)
          p2: [-0.042, -0.042, 0.0  ]  # back right  (M2)
          p3: [ 0.042, -0.042, 0.0  ]  # front right (M1)
      default_single_marker:
        offset: [0.0, -0.01, -0.04]
        points:
          p0: [0.0177184,0.0139654,0.0557585]
      mocap_deck:
        offset: [0.0, 0.0, -0.01]
        points:
          p0: [0.03, 0.0, 0.0] # front
          p1: [0.00, -0.03, 0.0] # right
          p2: [-0.015, 0.0, 0.0] # back
          p3: [0.00, 0.03, 0.0] # left
      medium_frame:
        offset: [0.0, 0.0, -0.03]
        points:
          p0: [-0.00896228,-0.000716753,0.0716129]
          p1: [-0.0156318,0.0997402,0.0508162]
          p2: [0.0461693,-0.0881012,0.0380672]
          p3: [-0.0789959,-0.0269793,0.0461144]
      big_frame:
        offset: [0.0, 0.0, -0.06]
        points:
          p0: [0.0558163,-0.00196302,0.0945539]
          p1: [-0.0113941,0.00945842,0.0984811]
          p2: [-0.0306277,0.0514879,0.0520456]
          p3: [0.0535816,-0.0400775,0.0432799]

    dynamics_configurations:
      default:
        max_velocity: [2, 2, 3] # m/s
        max_angular_velocity: [20, 20, 10] # rad/s
        max_roll: 1.4 #rad
        max_pitch: 1.4 #rad
        max_fitness_score: 0.001

    # Rigid bodies will be automatically generated by the launch file
    # rigid_bodies:

I have also had some other issues regarding the optitrack motion capture in #323 but this should be unrelated.

When looking at the data published over the poses topic, it looks reasonable when manually moving the drone around.

Consequently, the questions I have are:

1. What might be causing this behaviour and what can I do to fix it?
2. I have created a rigid body asset in Motive and am assuming that for my given config that the marker configuration does not particularly matter and does not need to line up with what is set in the marker_configurations . Is this correct?
3. If there are any other resources/issues/discussions which might be relevant, please do let me know!

Cheers

[mwlock]

Just for the sake of comparing the motion capture to the onboard EKF without motion capture data, I commented out the publishing of the motion capture to the CF and plotted the results for a takeoff and land. Seems close enough to assume there is no issue wih the motion capture data?

[mwlock]

After re-enabling the motion capture these are the results...

[whoenig]

First to your initial questions:

1. It could be related to "axis flips", i.e., Motive might track the positions well, but flips the axis randomly if the marker configuration is not unique. To test that, you can repeat your experiment but plot roll/pitch/yaw angles from the motion capture. Another option is to not fuse the orientation in the EKF (requires a firmware change).
2. Correct
3. You could try changing the EKF tuning, i.e., increase especially extQuatStdDev

 locSrv:
      extPosStdDev: 1e-3
      extQuatStdDev: 0.5e-1

Also, using librigidbodytracker or single marker tracking can help if Motive is indeed tracking poorly.

[mwlock]

Thank you for your suggestions!

I have ovserved the "axis flips" in Motive. I find the behaviour a little strange since it is the only active marker configuration. Nevertheless I will take a deeper look and repeat the experiment as per your suggestion.

[whoenig]

Motive is tracking globally, not frame-by-frame. If there is a motion filter in your Motive version it could avoid the axis flips. Otherwise relying on librigidbodytracking is better because it is using a motion filter.

[mwlock]

A different arrangement of the markers improved the axis flips dramatically and the control worked nicely after that. It still does occur occasionally so I would like to move to librigidbodytracking as you suggest.

I am unfortunately not having any luck with this 😅 The pointcloud topic is publishing but it seems the data field is empty. I have made sure that there are no "assets" defined in motive. Any thoughts as to what the issue might be?

[whoenig]

If you see markers in Motive, you might need to adjust the streaming settings (where you can enable/disable streaming of all kinds of data separately). Make sure to enable "unlabeled markers" as well as "labeled markers".

[sesem738]

Were you able to fix this problem? I am also suffering from the same thing