Adding a simple mocap example with multiple cf/rigid bodies

[JacopoPan]

I thought it could be useful to add a very simple extension of mocap_hl_commander.py to connect to multiple Crazyflies, even just for debugging purposes

• only sending positions from the mocap system
• using the motion commander as well
• tested with optitrack

[ArisMorgens]

Hello @JacopoPan!

Thank you for your contribution! Based on #430 this is something that we wanted to add to the cflib examples. We'll soon give it a try with our Qualisys MoCap system and get back to you.

[JacopoPan]

Thanks! Let me know how it goes, this is a strip down version of a script I tested with OptiTrack a couple of weeks ago.

[ArisMorgens]

Hello @JacopoPan!

We tested your script on our Qualisys MoCap, but didn't manage to make it work properly. We started by flying a single Crazyflie but right after taking off, it keeps flying upwards. This is shown well in the attached plot.

Switching between commanders didn't help and I can verify that we're receiving accurate MoCap data.

Let us know if this is something that you've experienced before.

[JacopoPan]

mocap.mp4

no, this is what I see on Motive

in fairness, the first drone (top of the screen in the video) is supposed to flight up to 2m in the z-axis (line 91) (can you try to change those go_to and see if they have any impact?)

is the mapping between radio IDs and the string with the name of the rigid body (this is specified in motive for OptiTrack) mapped as in lines 120-132?

(If none of this works, let me know and I’ll run this code again to check if I can reproduce and fix the problem)

[ArisMorgens]

Hello @JacopoPan. I have tried simplifying the script as much as possible by removing the swarm class and switching between different commanders, so I ended up with a pretty simple script where one Crazyflie takes off at 0.3m and then lands using the MotionCommander. Even this didn't work and the Crazyflie started flying upwards.

The mapping between radio ID and the rigid body is correct and I'm getting accurate positioning data from our MoCap system. If this isn't something that you can reproduce with your setup, we'll investigate it further here, as it might be related to the motioncapture library.

URI = 'radio://0/30/2M/C0F1EC0F1E'

def run_sequence(scf):
    print('This is: ', scf._link_uri)
    with MotionCommander(scf) as motc:
        time.sleep(3)
        motc.land()

if __name__ == '__main__':
    cflib.crtp.init_drivers()

    # Uncomment the URIs to connect to
    uris = [
        URI,
        # 'radio://0/90/2M/E7E7E7E7E8',
        # 'radio://0/100/2M/E7E7E7E7E9',
    ]

    # Maps the URIs to the rigid-body names as streamed by, e.g., OptiTrack's Motive
    rbs = {
        URI: 'cf',
     #   'radio://0/90/2M/E7E7E7E7E8': 'E8',
     #   'radio://0/100/2M/E7E7E7E7E9': 'E9'
    }

    with SyncCrazyflie(uris[0], cf=Crazyflie(rw_cache='./cache')) as scf:
        active_rbs_cfs = {rbs[uris[0]]: scf.cf}
        mocap_thred = MocapWrapper(active_rbs_cfs)

        reset_estimator(scf)
        time.sleep(2)
        start_position_printing(scf)
        start_desired_position_printing(scf)
        arm(scf)
        run_sequence(scf)

    mocap_thred.close()  

[JacopoPan]

Thanks @ArisMorgens , I will test again this script next week and also report with a list of packages and versions in my venv.