Energy efficient trajectory planner

Summary

Planning the robot trajectory to perform a movement with minimum energy consumption.