cmdFullState yaw not working

[MauroPfister]

Hi

I am having trouble getting the cmdFullState command to work, especially the yaw part. It seems logic to me that pos and vel have to be consistent. However, I am not sure what omega needs to be if I want to control yaw. @jpreiss suggests here that omega can be set to zero, but I guess this is only true if yaw = 0?

I tried to turn a CF by 90 degrees (while hovering) with the code snippet below but it does not move/turn at all. Changing yaw_rate to degrees/s and having yaw and omega with different signs or units did not help.

t = 0
while t < 4.0:
    pos = np.array([0, 0, 0.5])
    vel = np.array([0, 0, 0])
    acc = np.array([0, 0, 0])

    yaw_rate = np.pi / 2
    yaw = yaw_rate * t
    omega = np.array([0, 0, yaw_rate])

    cf.cmdFullState(pos, vel, acc, yaw, omega)

    t += 0.05
    timeHelper.sleep(0.05)

During my tests I also found some incorrect docstrings:

• The yaw argument of cmdPosition() seems to be in degrees and not in radians.
• The yawRate argument of cmdVelocityWorld() seems to be positive in clockwise direction which does not match with the coordinate system of the CF in my opinion.

[jpreiss]

Which controller are you using?

[jpreiss]

Honestly, I would not be that surprised if there is a bug in our yaw control. The large majority of our experiments have been position control with 0 yaw all the time.

[MauroPfister]

Which controller are you using?

I was using PID for which the yaw control did not work. Mellinger on the other hand works just fine. Does this look like a bug to you or is the combination simply not supported?

[jpreiss]

This combination should be supported. I implemented the cmdFullState packet with the Mellinger controller in mind. It is possible that I only tested it with the Mellinger controller. Failure with PID is probably coming from something simple like a sign error rather than anything fundamental.

[MauroPfister]

Since the Mellinger controller anyways seems to be more stable this is not a big issue. Would still be great if it worked though. Do you have time to investigate or give me a hint where to look?