Motion Planning

class Trajectory

A robot’s trajectory as a list of positions, velocities and accelerations at specific times.

The Trajectory class represents a sequence of kinematic states of a robot over a specified duration. It maintains lists of positions, velocities, and accelerations at particular time stamps.

Public Functions

inline explicit Trajectory(size_t dofs)

Create an empty trajectory with the given degrees of freedom.

Parameters:

dofs – The degrees of freedom of the joint space.

inline explicit Trajectory()

Default constructor.

inline size_t size() const

The number of time steps within the trajectory.

Returns:

The number of time steps within the trajectory.

State front() const

Access the first state at t=0 of the trajectory.

Returns:

The first state at t=0 of the trajectory.

State back() const

Access the last state at t=duration of the trajectory.

Returns:

The last state at t=duration of the trajectory.

void at_time(double time, Config &new_position, Config &new_velocity, Config &new_acceleration) const

Get the kinematic state at a given time. Make sure that the output arguments have enough memory.

Parameters:

  • time – The time at which to get the kinematic state.

  • new_position – [out] The joint position at the given time.

  • new_velocity – [out] The joint velocity at the given time.

  • new_acceleration – [out] The joint acceleration at the given time.

Config get_min_position() const

Get the minimum position along the trajectory for each degree of freedom individually.

Returns:

Config The minimum position value for each degree of freedom across the trajectory.

Config get_max_position() const

Get the maximum position along the trajectory for each degree of freedom individually.

Returns:

Config The maximum position value for each degree of freedom across the trajectory.

Config get_min_velocity() const

Get the minimum velocity along the trajectory for each degree of freedom individually.

Returns:

Config The minimum velocity value for each degree of freedom across the trajectory.

Config get_max_velocity() const

Get the maximum velocity along the trajectory for each degree of freedom individually.

Returns:

Config The maximum velocity value for each degree of freedom across the trajectory.

Config get_min_acceleration() const

Get the minimum acceleration along the trajectory for each degree of freedom individually.

Returns:

Config The minimum acceleration value for each degree of freedom across the trajectory.

Config get_max_acceleration() const

Get the maximum acceleration along the trajectory for each degree of freedom individually.

Returns:

Config The maximum acceleration value for each degree of freedom across the trajectory.

size_t get_step_closest_to(const Config &position) const

Get step at which the trajectory is closest (in terms of the L2 norm in joint space) to the reference position.

Returns:

size_t The step index of the closest position.

void update_first_position(const Config &joint_position)

Update the first position of the trajectory.

Parameters:

joint_position – The new position to set at the start of the trajectory.

Trajectory reverse() const

Reverse the trajectory’s start and goal.

Returns:

Trajectory A new trajectory with the start and end points reversed.

void append(const Trajectory &other)

Append another trajectory to the current one.

Parameters:

other – The trajectory to append.

Trajectory &operator+=(const Trajectory &other)

Append another trajectory to the current one using the += operator.

Parameters:

other – The trajectory to append.

Returns:

Trajectory& Reference to the updated trajectory.

Trajectory slice(size_t start, size_t steps) const

Slice a trajectory starting from step start for a length of steps.

Parameters:

  • start – The starting index of the slice.

  • steps – The number of steps to include in the slice.

Returns:

Trajectory A new trajectory containing the specified slice of the original.

std::vector<Config> filter_path(const Config &max_distance) const

Filter a path of sparse waypoints from the trajectory.

The path has a maximum distance per degree of freedom between the linear interpolation of the sparse waypoints and the original trajectory.

Parameters:

max_distance – The maximum allowable distance between joint positions.

Returns:

std::vector<Config> A list of sparse waypoints filtered from the trajectory.

std::string to_json() const

Serializes a trajectory to a json string.

void to_json_file(const std::filesystem::path &file) const

Saves a trajectory to a *.json file.

Parameters:

file – The path to the *.json file.

std::string as_table() const

To pretty print the trajectory as a table of positions.

Public Members

std::string id

Field for identifying trajectories (for the user)

std::string motion

Name of the motion this trajectory was planned for.

size_t degrees_of_freedom

The degrees of freedom (e.g. axis) of the trajectory.

double duration = {0.0}

The total duration in [s].

std::vector<double> times

The exact time stamps for the position, velocity, and acceleration values. The times will usually be sampled at the delta_time distance of the Planner class, but might deviate at the final step.

std::vector<Config> positions

The joint positions along the trajectory.

std::vector<Config> velocities

The joint velocities along the trajectory.

std::vector<Config> accelerations

The joint accelerations along the trajectory.

Public Static Functions

static Trajectory from_json(const std::string &json)

Loads a trajectory from a json string.

static Trajectory from_json_file(const std::filesystem::path &file)

Loads a trajectory from a *.json file.

Parameters:

file – The path to the *.json file.

Returns:

Trajectory The loaded trajectory.

class Planner

Planning motions for robots.

The Planner class is is the core of the Jacobi Motion library. It is responsible for computing robot trajectories within a given environment. It provides various parameters for configuring the planning process, including resolution, optimization settings, and compute budgets. The class also supports loading and managing motion plans.

Public Functions

explicit Planner(std::shared_ptr<Environment> environment, double delta_time)

Create a planner with an environment and a specific delta time parameter.

Parameters:

  • environment – The environment to plan the robot motions in.

  • delta_time – The time step for sampling the trajectories in [s].

explicit Planner(std::shared_ptr<Robot> robot, double delta_time)

Create a planner with the robot inside an empty environment and a specific delta time parameter.

Parameters:

  • robot – The robot to plan the motions for.

  • delta_time – The time step for sampling the trajectories in [s].

explicit Planner(std::shared_ptr<Environment> environment)

Create a planner with an environment.

Parameters:

environment – The environment to plan the robot motions in.

explicit Planner(std::shared_ptr<Robot> robot)

Create a planner with the robot inside an empty environment.

Parameters:

robot – The robot to plan the motions for.

void set_seed(std::optional<unsigned int> seed)

Set the seed of the planner’s random number generator.

Parameters:

seed – The seed to set. If no seed is provided, the generator will be seeded with a random value.

void add_motion(const AnyMotion &motion)

Add (or update when name already exists) a motion to the planner.

Parameters:

motion – The motion to add, can be of any type.

AnyMotion get_motion(const std::string &name) const

Get a motion by its name.

Parameters:

name – The name of the motion to retrieve.

Returns:

The AnyMotion object associated with the given name.

void load_motion_plan(const std::filesystem::path &file)

Load a *.jacobi-plan motion plan for accelerating the planning calculation.

Parameters:

file – The path to the *.jacobi-plan file to load.

std::optional<Trajectory> transfer_trajectory(const Trajectory &trajectory, const std::shared_ptr<RobotArm> &robot_from, const std::shared_ptr<RobotArm> &robot_to, const Frame &offset = Frame::Identity()) const

Transfers a trajectory from one robot to another.

Calculate a trajectory for another robot that follows the TCP of the original robot given the trajectory. This method does not check for constraints of the new robot.

Parameters:

  • trajectory – The trajectory to follow.

  • robot_from – The original robot to transfer from.

  • robot_to – The new robot to transfer to.

  • offset – Optional offset between the from and to robot’s TCP.

Returns:

The transferred trajectory or std::nullopt if the planning failed.

std::optional<Trajectory> plan(const Config &start, const Config &goal)

Plans a time-optimized, collision-free, and jerk-limited motion from start configuration to goal configuration.

Parameters:

  • start – The start configuration of the motion.

  • goal – The goal configuration of the motion.

Returns:

The computed trajectory or std::nullopt if the planning failed.

std::optional<Trajectory> plan(const Point &start, const Point &goal)

Plans a time-optimized, collision-free, and jerk-limited motion from start to goal.

Parameters:

  • start – The start point of the motion.

  • goal – The goal point of the motion.

Returns:

The computed trajectory or std::nullopt if the planning failed.

std::optional<Trajectory> plan(const std::string &name, const std::optional<ExactPoint> &start = std::nullopt, const std::optional<ExactPoint> &goal = std::nullopt)

Plans a time-optimized, collision-free, and jerk-limited motion given the motion name. In case the motion was specified by a start or goal region, the respective exact start or goal positions needs to be passed.

Parameters:

  • name – The name of the motion to plan.

  • start – The exact start position of the motion.

  • goal – The exact goal position of the motion.

Returns:

The computed trajectory or std::nullopt if the planning failed.

std::optional<Trajectory> plan(const Motion &motion, const std::optional<ExactPoint> &start = std::nullopt, const std::optional<ExactPoint> &goal = std::nullopt)

Plans a collision-free point-to-point motion.

Parameters:

  • motion – The motion to plan.

  • start – The exact start position of the motion.

  • goal – The exact goal position of the motion.

Returns:

The computed trajectory or std::nullopt if the planning failed.

std::optional<Trajectory> plan(const LinearMotion &motion)

Plans a linear motion.

Parameters:

motion – The linear motion to plan.

Returns:

The computed trajectory or std::nullopt if the planning failed.

std::optional<Trajectory> plan(const LowLevelMotion &motion)

Plans a low-level motion.

Parameters:

motion – The low-level motion to plan.

Returns:

The computed trajectory or std::nullopt if the planning failed.

std::optional<Trajectory> plan(const PathFollowingMotion &motion)

Plans a path-following motion.

Parameters:

motion – The path-following motion to plan.

Returns:

The computed trajectory or std::nullopt if the planning failed.

std::optional<std::vector<Trajectory>> plan(const std::vector<AnyMotion> &motions)

Plans a feasible sequence of motions.

Parameters:

motions – The sequence of motions to plan.

Returns:

The computed sequence of trajectories or std::nullopt if the planning failed.

Public Members

std::shared_ptr<Environment> environment

The current environment to plan robot motions in.

double delta_time

The time step for sampling the trajectories in [s]. Usually, this should correspond to the control rate of the robot.

Result last_calculation_result = {Result::Working}

Result of the last trajectory computation.

double last_calculation_duration = {0.0}

The calculation duration of the last full trajectory computation.

double pre_eps_steering = {1000.0}

Steering epsilon in the pre-planning stage [rad].

double pre_eps_collision = {3.33}

Resolution of the collision checking in the pre-planning stage [rad].

double pre_max_steps = {16 * 1024}

Maximum number of steps in the pre-planning stage before a solution is not found.

size_t pre_optimization_steps = {512}

Number of samples for optimization after finding a solution in the pre-plannning stage.

double initial_perturbation_scale = {0.04}

Initial perturbation for the trajectory optimization.

size_t perturbation_change_steps = {256}

Steps without improvement after which the perturbation scale is adapted.

double perturbation_scale_change = {1e-2}

Change of the perturbation if no improvement could be found recently.

size_t max_optimization_steps = {5 * 1024}

Maximum number of optimization steps.

size_t max_break_steps = {1024}

Max number of steps without improvement before early stopping.

double meaningful_loss_improvement = {1e-2}

A meaningful relative improvement to avoid stopping.

std::optional<float> min_calculation_duration

The minimum compute budget (that the planner can use regardless)

std::optional<float> max_calculation_duration

The maximum compute budget (that won’t be exceeded)

Public Static Functions

static std::shared_ptr<Planner> load_from_project_file(const std::filesystem::path &file)

Loads a planner from a project file.

Parameters:

file – The path to the project file.

Returns:

A shared pointer to the loaded Planner object.

static std::shared_ptr<Planner> load_from_studio(const std::string &name)

Loads a planner from a Studio project. Make sure to have the access token set as an environment variable.

Parameters:

name – The name of the Studio project.

Returns:

A shared pointer to the loaded Planner object.