Sequence Planning¶
Beyond planning single motions, the Planner class also allows to plan a feasible sequence of motions. This is useful for complex tasks that require multiple steps, such as pick-and-place operations or when planning motions for multiple robots operating in the same environment.
First, let’s construct an Environment object with two robots and the associated planner:
robots = {robot1, robot2}
environment = Environment(robots, 0.03)
planner = Planner(environment)
std::set<std::shared_ptr<Robot>> robots {robot1, robot2};
Environment environment {robots, 0.03};
Planner planner {environment};
Next, we define a sequence of motions that we want to plan. Each motion can be either of a Motion or a LinearMotion type, which we described in the previous sections. We can then create a list of motions:
motion1 = Motion('motion1', robot1, start1, waypoint1)
motion2 = Motion('motion2', robot2, start2, goal2)
motion3 = LinearMotion('motion3', robot1, waypoint1, goal1)
motions = [motion1, motion2, motion3]
Motion motion1 {"motion1", robot1, start1, waypoint1};
Motion motion2 {"motion2", robot2, start2, goal2};
LinearMotion motion3 {"motion3", robot1, waypoint1, goal1};
std::vector<Motion> motions {motion1, motion2, motion3};
Finally, we can plan the sequence of motions by invoking the plan method with the list of motions:
trajectories = planner.plan(motions)
auto trajectories = planner.plan(motions);
The planner will return a list of trajectories that are optimized for the given sequence of motions and can be sequentially executed on the associated robots.