Drifting while rotating

[pulver22]

Hi,

I'm trying to assign goal position to the drone using the c moveByRel X Y Z yaw command and I noticed the following behaviour: if the movements needed to be done are pure translation, the drone reaches the desired position without big efforts; but, in the case it has also to rotate, it drifts in the opposite direction of the rotation and take it back on track requires some time.

E.g.: suppose the goal position is ( 0.6, 2.5, -0.70, -30) wrt to the drone. It starts moving trying to assume the right orientation as first, meanwhile it reduces the offset on the three axis. What I always noticed is that, after reducing the offset on X (from 0.6 to 0) it drifts increasing it (from 0 to -0.5, for example). Therefore it spends time to realign again on X.

I read somewhere that this can be a drawback of a monocular system as PTAM but I'm not sure. Do you think is it possible to reduce this behaviour and obtain a smooth movements from the starting position to a goal one?

[kbogert]

You can turn off noise in the cvg_sim_gazebo_plugins/urdf/quadrotor_simple_controller.urdf.xacro file, but I'm afraid drift is a fact of life with these machines.

One thing that might be throwing you off is that the yaw is specified in degrees, and for small values it might be better to just tell the drone to hover than to try to obtain that level of accuracy.

[pulver22]

About the yaw, I already decided to hover if yaw is in [-10,10].

I can try to turn off the noise but I think that this problem can appear once I'll do test in real life.

EDIT: turning off the noise doesn't seem solving the issue!

[kbogert]

With the drift turned off I'm not seeing this. At takeoff the simulated drone does slowly drift due to there being no control applied but once autopilot is engaged (due to either a goto or moveByRel command) the drone slowly oscillates around the requested point. I think this small oscillation can be eliminated by playing with the autopilot parameters if required.

Can you paste a command sequence that reliably reproduces the problem you're seeing?

[pulver22]

The following is the command sequence generated by my node: c moveByRel 1.12310791 4.16973019 -1.123945 -42.80891 c moveByRel 1.30998707 3.93357062 -1.15005434 -11.359827 c moveByRel 0.930803955 3.92632008 -0.955585361 0 c moveByRel 0.765609801 3.68364859 -0.847805858 11.8716364 c moveByRel 0.790131271 3.41124988 -0.759122312 0 c moveByRel 0.307481408 3.33210754 -0.731468379 0 c moveByRel 0 3.1415813 -0.625943959 13.4682875 c moveByRel -0.215701848 3.00566268 -0.630780339 0 c moveByRel 0 2.93345094 -0.462048352 0 c moveByRel -0.173180073 2.76431966 -0.46337527 0 c moveByRel -0.307157815 2.62240982 -0.329265684 0 c moveByRel -0.403499901 2.43094277 -0.319666624 0 c moveByRel 0 2.55101466 -0.136655405 0 c moveByRel 0 2.59509754 -0.101506442 0 c moveByRel 0 2.49795294 0 0 c moveByRel -0.135041952 2.37728 0.146000385 0 c moveByRel 0 2.26730824 0 0 c moveByRel -0.226278678 2.12970352 0.240405649 0 c moveByRel -0.268768311 2.03871989 0.185594842 0 c moveByRel -0.275961727 1.94140041 0.124990873 0 c moveByRel -0.300729573 1.64083409 -0.218532175 0 c moveByRel 0 1.40198934 0 0 c moveByRel -0.224020123 1.28997803 0 11.4324026 c moveByRel -0.251054943 1.22679007 0 0 c moveByRel -0.20936957 1.17039514 0 0 c moveByRel -0.182115644 1.1187396 0 0 c moveByRel -0.156094879 1.0704304 0 0 c moveByRel 0 1.01982522 0 0 c moveByRel -0.116760649 0.961421967 0 0 c moveByRel 0 0.916346252 0 0 c moveByRel 0 0.865539134 0 0 c moveByRel 0 0.824565649 0 0 c moveByRel 0 0.78286171 0 0 c moveByRel 0 0.74810046 0 0 c moveByRel 0 0.711458623 0 0 c moveByRel 0 0.678158641 0 0 c moveByRel 0 0.645382285 0 0 c moveByRel 0 0.611970901 0 0 [ INFO] [1463580912.806161681, 2095.684000000]: Destination reached c moveByRel 0 0 0 0 `

At a frequency of 1Hz, my node send the following command plus a 'moveByRel' one : "c clearCommands"; "c autoInit 500 800"; "setReference $POSE$"; "setMaxControl 1"; "setInitialReachDist 0.1"; "setStayWithinDist 0.1"; "setStayTime 0.5";

It's also possible I am using the autopilot in the wrong way but I have a smooth behaviour with the bottomcamera, so I don't know what to think...

[kbogert]

Oh I see, most of those commands are only needed at initialization.

Once you've sent them the first time, afterwards just send a clearCommands followed by the moveByRel

[pulver22]

@kbogert can you explain me how the drone behaviour shouls change if I set kp_rp and xy_damping_factor params with different values?

[kbogert]

Going by the linear damping in this wikipedia page: https://en.wikipedia.org/wiki/Damping

k_rp is k in those equations, controlling the strength of the simulated spring system. Increasing this will cause the drone to bank more sharply and in general move faster

xy_damping_factor is zeta, the damping ratio. Right now the spring system is underdamped, causing the drone to slightly overshoot its target and oscillate back a bit (which is desirable in reality since other forces like friction usually act to slow the drone a bit and the forces produced always lag those requested due to the time it takes to rotate to the needed angles). Setting this to 1.0 causes the system to be critically damped, reaching the target in the minimum amount of time. > 1.0 is overdamped, slowing its approach to the target.

One last parameter to play with is the droneMassInKilos, I believe in tum_simulator this is actually 1.5kilos but it is more like 0.5kilos in reality. As you can see from the wikipedia page this value is pretty important but you can get away with some inaccuracy. Increasing this value without also increasing K will cause the drone to move much more slowly in all ways, which I found useful for debugging. Don't set it too low though or the system will be unstable (the drone can't rotate fast enough to simulate the resulting spring system)