Bugfix and reformulation

src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp

src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h

src/ZMPRefTrajectoryGeneration/door.cpp

src/ZMPRefTrajectoryGeneration/door.hh

src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp

src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh