search

MatthewPeterKelly / OptimTraj

A trajectory optimization library for Matlab
MIT License
598 stars 207 forks source link

Problem with z state in quadRotor3D #43

Closed pedropereira97 closed 3 years ago

pedropereira97 commented 3 years ago

Hi, Excuse my question but I have tried to run MAIN.m in the quadRotor3D demo and after analysing the result of the optimizer I found that the position z shows strange behavior at least for me.

Basically when I look at the velocity in z and at the position in z they dont make sense because the variation of z is a lot higher than its velocity. In the repository example, the z position goes from 0 to 25 m in less than a second but the velocity stays between -2 and 2 m/s.

Another problem I found is when my initial z position set in inicial state is different than zero, the initial z position of the solution is completly wrong. For example, I tried setting my initial z position to 3 m and after solving the initial z position of the solution was 170 m.

I have tried to analyse the system dynamics but nothing looks wrong to me so if you could help me with this I would be thankful to you.

MatthewPeterKelly commented 3 years ago

Hmm... That sounds like a bug. I don't have time to dig into it right now, but it would be great to understand what is going on here.

@cjmcgreal -- any chance that you would have a few minutes to take a look at this?

cjmcgreal commented 3 years ago

yeah, I'll take a look. Thanks @MatthewPeterKelly!

cjmcgreal commented 3 years ago

@pedropereira97 good find! Thanks for pointing this out. I think I have this fixed and have opened a PR. Are you working with the PX4 stack?