PositionHlCommander Localization system

Firmware/software/electronics/mechanics
Naidala
Beginner
Posts: 14
Joined: Fri Dec 11, 2020 9:10 am

PositionHlCommander Localization system

Post by Naidala »

position_commander_demo is not working, it does run the script (if I put some print here and there it does execute them), but it does not fly, not even start the motors.
I'm reading the script requires some kind of Localization system, but I have a Vicon MOCAP. Should I configure it someway, in order for the CF to "know" it exists... Up to now, I have just executed the code as it is in https://github.com/bitcraze/crazyflie-l ... er_demo.py
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: PositionHlCommander Localization system

Post by arnaud »

When using a mocap system you must send the Crazyflie position in your script to the Crazyflie.

We have an example of that for Qualisys (https://github.com/bitcraze/crazyflie-l ... mmander.py) and there is a ticket discussing of a Vicon version of it (https://github.com/bitcraze/crazyflie-l ... issues/151). However there is still no Vicon example in the lib.

My suggestion would be to look at the Crazyswarm project: https://crazyswarm.readthedocs.io/en/latest/. It is based on ROS and has been designed to run Crazyflies in a MOCAP system. Vicon is one of the system supported (it was the one Crazyswarm was developed on).
Naidala
Beginner
Posts: 14
Joined: Fri Dec 11, 2020 9:10 am

Re: PositionHlCommander Localization system

Post by Naidala »

Thanks! I have used send_extpose with measurements from Vicon in this way:

Code: Select all

	with LogEstimationManager() as log_est:
		with SyncCrazyflie(URI) as scf:
			cf = scf.cf
			log_est.simple_log_async(cf)
			log_est.reset_estimator(cf)
			vicon = ViconManager()
			time.sleep(0.5)
			init_pos, V2G_rot = vicon.get_drone_pose()
			with PositionHlCommander(
					scf,
					x=init_pos[0], y=init_pos[1], z=init_pos[2],
					default_velocity=0.3,
					default_height=0.5,
					controller=PositionHlCommander.CONTROLLER_PID) as pc:
				for setpoint in sequence_V:
					for i in range(20):
						pos_drone, quat_drone = vicon.get_drone_pose(V2G_rot)
						pos_est, att_est = log_est.get_estimation()
						cf.extpos.send_extpose(pos_drone[0], pos_drone[1], pos_drone[2],
												   quat_drone[0], quat_drone[1], quat_drone[2], quat_drone[3])
						pc.go_to(setpoint[0], setpoint[1], setpoint[2])
						time.sleep(0.1)
	
where LogEstimationManager and ViconManager are just two classes I defined to store the functions used for logging and vicon.

Isn't this sufficient?
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: PositionHlCommander Localization system

Post by arnaud »

Something like that should work, yes. The easiest might be to send the position in a separate thread, this way you can separate the code that drives the Crazyflie and the code that update the position and you will be able to update the position more often than 10 times per seconds (100Hz should work without problems).

If you get a simple example working, we will be happy if you want to contribute it as an example to the lib, so do not hesitate to do a PR :-).
Naidala
Beginner
Posts: 14
Joined: Fri Dec 11, 2020 9:10 am

Re: PositionHlCommander Localization system

Post by Naidala »

I upgraded the firmware and now it seems to be working. I only have one more question: as far as I understood, the position is set in the init of PositionHlCommander class, but then I see no other function changing it. Will it be automatically updated on a low level at every new send_extpose command?
send the position in a separate thread, this way you can separate the code that drives the Crazyflie and the code that update the position and you will be able to update the position more often
That's a great idea, thanks! As soon as the exercise is finished we will share it in the lib ;)
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: PositionHlCommander Localization system

Post by kristoffer »

as far as I understood, the position is set in the init of PositionHlCommander class, but then I see no other function changing it. Will it be automatically updated on a low level at every new send_extpose command?
The PositionHlCommander uses the initial position and adds any motion to it to keep track of the current position. The position is required to implement relative motion commands such as left() and forward() since the absolute position as always sent to the Crazyflie in this implementation.
Naidala
Beginner
Posts: 14
Joined: Fri Dec 11, 2020 9:10 am

Re: PositionHlCommander Localization system

Post by Naidala »

Forgive me, I don't know if I understood your answer correctly. I see the go_to() command inside PositionHlCommander sets the absolute desired position x as the current position self._x at the end of a sleep. Is that what you mean when you say it "adds any motion to it to keep track of the current position"?
My question is: What's the role of Kalman estimation in all that? How does send_extpose() affect the setting self._x = x? The current position self._x should not simply be set as the desired one x, but should be improved with the aid of external measurement, I guess... Does the firmware somehow update the value of self._x inside PositionHlCommander after the send_extpose() and the Kalman update?

One more thing, why do you say
The position is required to implement relative motion commands such as left() and forward()
can't I ask for an absolute position with the go_to() command?
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: PositionHlCommander Localization system

Post by kristoffer »

Sorry for the slightly confuse and vague information!

self._x, self._y and self._z contains the position that the PositionHlCommander believe the Crazyflie is at, it is updated whenever you call a method that moves the CF. You can get the position by calling get_position()

Yes, go_to() expects an absolute position.
Methods like left() and back() use a distance and are thus relative to the current position, some thing like "go left 1 meter from the current position"

The estimator is not really directly affected by the high level commander. The high level commander generates set points that are used by the controller to move the CF along the desired trajectory to a destination.

If you want to start your CF at some position that is not the origin the correct procedure would be:
1. Let the position estimator know where the CF is located by setting the kalman.initialX, kalman.initialY and kalman.initialZ parameters (assuming you are using the kalman estimator).
2. Reset the estimator by setting kalman.resetEstimation. It will now set the internal state to the initial position and fuse information from sensors to maintain an estimate of the position
3. Start the PositionHlCommander and provide the same position
4. Take off and do cool stuff!

Depending on which positioning system you use, 1 and 2 can often be achieved by just putting the CF on the floor and wait a bit to let the kalman filter converge to the correct position. There are some cases though where it might not work and the kalman estimator needs an initial guess to find a solution.
The PositionHlCommander needs to know where it is though. If the correct position is not set it will assume it is in the origin and a call to left(1.0) will be equal to calling go_to(0.0, 1.0) which probably is not the expected behaviour.

There is no "magic" going on behind the scenes, what you see in the code is what is executed. PositionHlCommander (python) => high level commander (in CF) => set points to controller (in CF). In parallel the estimator is trying to maintain an estimate of the current position based on sensor input.
Naidala
Beginner
Posts: 14
Joined: Fri Dec 11, 2020 9:10 am

Re: PositionHlCommander Localization system

Post by Naidala »

Thanks, that's better, I appreciate it. I see no upvote button but thanks, really!
Kaiser1401
Beginner
Posts: 11
Joined: Tue Apr 21, 2020 11:36 am

Re: PositionHlCommander Localization system

Post by Kaiser1401 »

Sorry for bringin this old thread up.

I used to set an initial state estimate according to steps 1 and 2 as mentioned by kristoffer.

kristoffer wrote: Tue May 11, 2021 8:10 am ...

If you want to start your CF at some position that is not the origin the correct procedure would be:
1. Let the position estimator know where the CF is located by setting the kalman.initialX, kalman.initialY and kalman.initialZ parameters (assuming you are using the kalman estimator).
2. Reset the estimator by setting kalman.resetEstimation. It will now set the internal state to the initial position and fuse information from sensors to maintain an estimate of the position
3. ...
4. ...

...
unfortunately commit d8d487 "destroys" that behaviour as the function call to 'kalmanCoreDefaultParams' in estimator_kalman.c:437 overwrites the estimate with zeros now instead of taking the 'InitalX' ... parameters.

Is this new wanted behaviour?
Is there a new way to overwrite the state estimate?
Should I have made an issue for this instead?

-Klaus
Post Reply