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.

[ArisMorgens]

Hello @JacopoPan! I have some updates. After looking at the other MoCap examples I found out that these lines are missing from your example. I added this function into your example and now the Crazyflies can take off and perform the sequence.

def activate_kalman_estimator(cf):
    cf.param.set_value('stabilizer.estimator', '2')

    # Set the std deviation for the quaternion data pushed into the
    # kalman filter. The default value seems to be a bit too low.
    cf.param.set_value('locSrv.extQuatStdDev', 0.06)

[JacopoPan]

Thanks for looking into this @ArisMorgens !
Idk if it's something I accidentally deleted in cleaning up the code for this PR (in that case sorry for wasting your time like this) or is related to the type of motion capture system.

Where do you call activate_kalman_estimator? If you can push the example you have flown, I'll test as-is with OptiTrack tomorrow (so we are sure that the same scrip works on qualisys and optitrack)

[ArisMorgens]

Hello @JacopoPan! This could be something specific for the qualisys MoCap. No worries at all, at the very least, I got the chance to better understand the MoCap system :).

I just pushed a simple example that worked for me. Give it a try if you have time and let me know if it works well for you too.

[ArisMorgens]

I will also start pushing some commits to reshape a bit your initial example, so it fits better to our format.
I think that’s easier than leaving review comments in your code that you’ll need to address later. However, if you’d prefer comments instead, just let me know.

[JacopoPan]

I will also start pushing some commits to reshape a bit your initial example, so it fits better to our format. I think that’s easier than leaving review comments in your code that you’ll need to address later. However, if you’d prefer comments instead, just let me know.

👍 absolutely feel free to add anything that you deem appropriate

[JacopoPan]

Sorry for the delay @ArisMorgens

I just pushed on commit 7bf438b exactly how I last flew the 2 examples

I made the following modifications

• the mocap variables (type, host_name) are my own (but everyone will have to change them according to their use, feel free to revert/set the defaults you prefer)
• I revised a bit how uris and rbs are created (I think it's more flexible now)
• My URIs are on different channels (80, 90, 100) and I use a single Crazyradio 2.0 (I removed your comment on using a single channel because I'm unsure about it, that doesn't seem to be the case for me)
• In each script, I only left uncommented the first crazyflie (I think it's safer/better to always start with just one before flying more)

[ArisMorgens]

Hello @JacopoPan!

Thank you for testing! In 4c84f36 I did some minor changes to make the examples pass the CI test and in da0b4b3 I changed the radio channels. Even though having the drones in different channels works, it is more "expensive" and could potentially slower the communication.

That said, I think this PR is ready to be merged! Thank you so much for your contribution and the time you spent on it.

[JacopoPan]

Happy to help! I also have no further comments