An Efficient Computation for Energy Optimization of Robot Trajectory