timor.utilities.trajectory

Classes

Trajectory

Holds an ordered set of states (can be any one of these):

Functions

greedy_ik_trajectory(trajectory, robot[, retries])

Calculate an inverse kinematic trajectory for this end-effector trajectory greedily.

Module Contents

class timor.utilities.trajectory.Trajectory

Holds an ordered set of states (can be any one of these):

• robot configuration q + change

• poses

each with or without time information.

__add__(other)

Appends other trajectory to self.

Return type:

Trajectory

__eq__(other)

Check if two trajectories are equal.

Return type:

bool

__getitem__(item)

Get sub-trajectory via indexing.

Parameters:

item (Union[int, Iterable[int], slice])

Return type:

Trajectory

__getstate__()

Custom get state, e.g. for pickle / deepcopy.

__iter__()

Explicit iterator over trajectory.

__len__()

Return number of samples in this trajectory

Return type:

int

__post_init__()

Check consistency such as same size for t, q, dq, ddq, and / or pose.

__setstate__(state)

Custom set state, e.g. for pickle / deepcopy.

Parameters:

state (Dict)

_allowed_deviation_in_time: float = 1e-12

_ddq: timor.utilities.dtypes.Lazy | numpy.ndarray

static _deduce_package_dir(filepath, content)

Logic for deducing the package directory if the JSONable object is loaded from a file.

Parameters:

• filepath (pathlib.Path) – The path to the file.

• content (Union[Dict, List]) – The content of the file parsed into a dictionary.

Note:

This method can be overridden by subclasses to provide custom package directory resolution; e.g. Task.py.

Return type:

pathlib.Path

_dq: timor.utilities.dtypes.Lazy | numpy.ndarray

add_tolerance(tolerance)

Add a tolerance to each pose of this trajectory.

Parameters:

tolerance (Union[ToleranceBase, Iterable[ToleranceBase]]) – The tolerance to add; if iterable each Tolerance is added to pose with same index.

Return type:

Trajectory

property ddq: numpy.ndarray

The joint accelerations.

Return type:

numpy.ndarray

property dof

Number of degrees of freedom encoded by this trajectory.

property dq: numpy.ndarray

The joint velocities.

Return type:

numpy.ndarray

classmethod empty()

Create and return an empty, timed, q-based trajectory

Return type:

Trajectory

fk_trajectory(robot)

Calculate a forward kinematic trajectory for this joint trajectory.

I.e., execute this trajectory defining a joint space movement on the provided assembly and create a new trajectory of the end-effector movement and its poses over time

Parameters:

robot (timor.Module.RobotConvertible) – The robot to calculate the FKs with.

Returns:

The end-effector trajectory that follows the joint trajectory.

Return type:

Trajectory

classmethod from_json_data(d, **kwargs)

Load from dict

Parameters:

d (Dict)

Return type:

Trajectory

classmethod from_json_file(filepath, *args, **kwargs)

Factory method to load a class instance from a json file.

Parameters:

• filepath (Union[pathlib.Path, str]) – The path to the json file.

• args – Additional arguments to pass to the from_json_data factory method of the specific class.

• kwargs – Additional arguments to pass to the from_json_data factory method of the specific class.

classmethod from_json_string(s, *args, **kwargs)

Create from a json string.

Parameters:

s (str)

classmethod from_mstraj(via_points, dt, t_acc, qd_max, **kwargs)

Create multi-segment multi-axis trajectory.

See: https://petercorke.github.io/robotics-toolbox-python/arm_trajectory.html?highlight=mstraj

Parameters:

• via_points (numpy.array) – Via points to connect with PTP trajectory. Start at first row, stop at last

• dt (float) – sample time in seconds

• t_acc (float) – acceleration time per PTP segment

• qd_max (float) – maximum speed in each axis

• kwargs – Additional key word arguments piped directly into the mstraj function

Return type:

Trajectory

get_at_time(time)

Returns state at desired time.

Parameters:

time (float)

Return type:

Trajectory

goal2time: Dict[str, float]

property has_ddq: bool

Does this trajectory enforce a configuration

Return type:

bool

property has_dq: bool

Does this trajectory enforce a configuration

Return type:

bool

property has_goals: bool

Does this trajectory refer to goal2time

Return type:

bool

property has_poses: bool

Does this trajectory enforce a pose.

Return type:

bool

property has_q: bool

Does this trajectory enforce a configuration

Return type:

bool

property is_timed: bool

Does this trajectory enforce time.

Return type:

bool

plot(show_goals_reached=False, show_figure=True)

Plot this trajectory

Parameters:

• show_goals_reached (bool) – If set to true, the time at which goals are reached is highlighted in the plot

• show_figure (bool) – Toggle call to plt.show() which might disable some figure manipulations.

Returns:

Figure with a plot of this trajectory.

Return type:

matplotlib.pyplot.Figure

pose: numpy.ndarray | None = None

classmethod pose_interpolation(T_start, T_end, steps)

Create a trajectory that interpolates between two poses.

Parameters:

• T_start (Transformation) – The start pose.

• T_end (Transformation) – The end pose.

• steps (int) – The number of steps to interpolate.

Returns:

The interpolated trajectory.

Return type:

Trajectory

q: numpy.ndarray | None = None

classmethod stationary(time, *, q=None, pose=None, sample_time=None)

Create a trajectory that stays in place for time seconds.

Parameters:

• time (float) – Time period to stay at this configuration. If sample time is given will be rounded up to next multiple of sample time.

• q (numpy.array) – Configuration to stay in (mutually exclusive with pose).

• pose (timor.utilities.tolerated_pose.ToleratedPose) – Tolerated pose to stay in (mutually exclusive with configuration).

• sample_time (Optional[float]) – If given, the time at which to sample the stationary trajectory; otherwise only 0 and time are sampled.

Returns:

The stationary trajectory, composed of two configurations / pose and a zero velocity / acceleration

Return type:

Trajectory

t: numpy.ndarray | float | None = None

to_json_data()

Transform into a jsonable dict.

Return type:

Dict

to_json_file(save_at, *args, **kwargs)

Writes the instance to a json file.

Parameters:

save_at (Union[pathlib.Path, str]) – File location or folder to write the class to.

to_json_string()

Return the json string representation.

Return type:

str

visualize(viz, *, name_prefix=None, scale=0.33, num_markers=2, line_color=None, robot=None)

Visualize trajectory and add num_markers triads to show orientation.

Parameters:

• viz (pinocchio.visualize.MeshcatVisualizer) – The visualizer to show the trajectory in

• name_prefix (Optional[str]) – Name of the visualized trajectory.

• scale (float) – size of the orientation markers.

• num_markers (Optional[int]) – how many orientation markers to show.

• line_color (Optional[numpy.ndarray]) – An optional color (3x1 vector) for each line-segment (total len(self)-1 x 3); default red; if single color vector given is broadcasted to all line segments.

• robot (Optional[timor.Module.RobotConvertible]) – A robot to evaluate a joint space trajectory on; plots the end-effector movement

Return type:

None

timor.utilities.trajectory.greedy_ik_trajectory(trajectory, robot, retries=10, **ik_kwargs)

Calculate an inverse kinematic trajectory for this end-effector trajectory greedily.

Greedily as in path planning, use the first best IK guess and try to move along the trajectory continuing from it. On failure retries with another random initial IK guess.

Parameters:

• trajectory (Trajectory) – The end-effector trajectory to calculate the IKs for.

• robot (timor.Module.RobotConvertible) – The robot to calculate the IKs with.

• tolerance – The tolerance for the end-effector poses (global or per step).

• retries (int) – How many times to retry the IK calculation.

• ik_kwargs – Additional keyword arguments to pass to the IK solver; allow_random_restart will only affect finding the IK solution of the first step and ignored afterward.

Returns:

The joint trajectory that follows the end-effector trajectory.

Raises:

ValueError – If no valid IK trajectory could be found.

Return type:

Trajectory