CenterOfMassTrajectoryCommand command1 = new CenterOfMassTrajectoryCommand();
command1.getEuclideanTrajectory().addTrajectoryPoint(0.0, new Point3D(0.0, 0.0, 0.0), new Vector3D(0.0, 0.0, 0.0));
command1.getEuclideanTrajectory().addTrajectoryPoint(1.0, new Point3D(1.0, 1.0, 0.0), new Vector3D(1.5, 1.5, 0.0));
command1.getEuclideanTrajectory().addTrajectoryPoint(2.0, new Point3D(2.0, 2.0, 0.0), new Vector3D(0.0, 0.0, 0.0));
command1.getEuclideanTrajectory().setCommandId(0L);
handler.handleComTrajectory(command1);
CenterOfMassTrajectoryCommand command2 = new CenterOfMassTrajectoryCommand();
command2.getEuclideanTrajectory().addTrajectoryPoint(1.0, new Point3D(0.0, 0.0, 0.0), new Vector3D(0.0, 0.0, 0.0));
command2.getEuclideanTrajectory().addTrajectoryPoint(2.0, new Point3D(1.0, 1.0, 0.0), new Vector3D(1.5, 1.5, 0.0));
command2.getEuclideanTrajectory().addTrajectoryPoint(3.0, new Point3D(2.0, 2.0, 0.0), new Vector3D(0.0, 0.0, 0.0));
command1.getEuclideanTrajectory().setPreviousCommandId(0L);
command2.getEuclideanTrajectory().setExecutionMode(ExecutionMode.QUEUE);