https://5dok.net/document/4yr2w88z-minimum-energy-trajectory-planning-for-robotic-manipulators.html