Trajectory;Robots;Planning;Safety;Dynamics;Collision avoidance;Robot sensing systems;Motion planning;computer graphics;timed elastic band (TEB);homology class of trajectories