Trajectory planning systems and methods

A collaborative robot, comprising:

• a manipulator arm configured to move from an initial pose to a goal pose;
• and
• a controller configured to:
• determine a plurality of waypoints defining a path between the initial pose and the goal pose;
• determine a trajectory between the initial pose and the goal pose based on the waypoints and one or more trajectory criterion by (i) selecting, from among a plurality of independent predetermined basis functions, one or more basis functions corresponding to a dynamic constraint type of respective ones of the one or more trajectory criterion that are to be satisfied while traversing the trajectory between the initial pose and the goal pose, the respective ones of the one or more trajectory criterion that are to be satisfied comprising a minimization of velocity, acceleration, or jerk of the robot while traversing the trajectory, and (ii) forecasting, after the robot has moved from the initial pose, a future pose of the robot that satisfies the one or more trajectory criterion, and using the future pose as a starting pose of the robot for one of the plurality of waypoints along the trajectory;
• and
• generate a control signal to control the manipulator arm based on the trajectory.

A non-transitory computer-readable storage medium with an executable program stored thereon, that when executed, instructs a processor to perform a motion planning method, for an autonomous agent, comprising:

• determining a plurality of waypoints defining a path between an initial pose of a manipulator arm of the autonomous agent and a goal pose of the manipulator arm;
• determining a trajectory between the initial pose and the goal pose based on the waypoints and one or more trajectory criterion by (i) selecting, from among a plurality of independent predetermined basis functions, one or more basis functions corresponding to a dynamic constraint type of respective ones of the one or more trajectory criterion that are to be satisfied while traversing the trajectory between the initial pose and the goal pose, the respective ones of the one or more trajectory criterion that are to be satisfied comprising a minimization of velocity, acceleration, or jerk of the autonomous agent while traversing the trajectory, and (ii) forecasting, after the manipulator arm has moved from the initial pose, a future pose of the manipulator arm that satisfies the one or more trajectory criterion, and using the future pose as a starting pose of the manipulator arm for one of the plurality of waypoints along the trajectory;
• and
• generating a control signal to control the manipulator arm based on the trajectory.