klampt.model.trajectory module

Classes for loading, saving, evaluating, and operating on trajectories.

class klampt.model.trajectory.GeodesicTrajectory(geodesic, times=None, milestones=None)[source]

Bases: klampt.model.trajectory.Trajectory

A trajectory that performs interpolation on a GeodesicSpace. See klampt.geodesic for more information.

constructor()[source]
difference(a, b, u, dt)[source]
interpolate(a, b, u, dt)[source]
class klampt.model.trajectory.HermiteTrajectory(times=None, milestones=None, dmilestones=None)[source]

Bases: klampt.model.trajectory.Trajectory

A trajectory whose milestones are given in phase space (x,dx).

eval_config(t) returns the configuration, and eval_velocity(t) justs get the velocity, and to get acceleration, use eval_accel(t).

If you want to use one of these trajectories like a normal configuration-space trajectory, so that eval() returns a configuration, call self.configTrajectory()

If dmilestones is given, then milestones is interpreted as configurations and dmilestones is interpreted as velocities.

Otherwise, the milestones are interpreted as states (x,dx)

configTrajectory()[source]
constructor()[source]
difference(a, b, u, dt)[source]
eval_accel(t, endBehavior='halt')[source]

Returns just the acceleration component of the derivative

eval_config(t, endBehavior='halt')[source]

Returns just the configuration component of the result

eval_state(t, endBehavior='halt')[source]

Returns the (configuration,velocity) state at time t.

eval_velocity(t, endBehavior='halt')[source]

Returns just the velocity component of the result

interpolate(a, b, u, dt)[source]
makeSpline(waypointTrajectory, preventOvershoot=True)[source]

Computes natural velocities for a standard configuration- space Trajectory to make it smoother.

class klampt.model.trajectory.RobotTrajectory(robot, times=None, milestones=None)[source]

Bases: klampt.model.trajectory.Trajectory

A trajectory that performs interpolation according to the robot’s interpolation scheme.

constructor()[source]
difference(a, b, u, dt)[source]
getLinkTrajectory(link, discretization=None)[source]

Returns the SE3Trajectory corresponding to the link’s pose along the robot’s trajectory. If discretization = None, only the milestones are extracted. Otherwise, the piecewise linear approximation at dt = discretization is used.

interpolate(a, b, u, dt)[source]
class klampt.model.trajectory.SE3Trajectory(times=None, milestones=None)[source]

Bases: klampt.model.trajectory.GeodesicTrajectory

A trajectory that performs interpolation in SE3. Each milestone is a 12-D flattened klampt.se3 element (i.e., the concatenation of R + t for an (R,t) pair).

Constructor can take either a list of SE3 elements or 12-element vectors.

constructor()[source]
deriv_se3(t, endBehavior='halt')[source]

Returns the derivative as the derivatives of an SE3 element

eval_se3(t, endBehavior='halt')[source]

Returns an SE3 element

from_se3(T)[source]

Converts a klampt.se3 element to a state parameter vector

getPositionTrajectory(localPt=None)[source]

Returns a Trajectory describing the movement of the given local point localPt (or the origin, if none is provided).

getRotationTrajectory()[source]

Returns an SO3Trajectory describing the rotation trajectory.

postTransform(T)[source]

Postmultiplies every transform in here by the se3 element T. In other words, if T transforms a local frame F to frame F’, this method converts this SE3Trajectory from describing how F’ moves to how F moves.

preTransform(T)[source]

Premultiplies every transform in here by the se3 element T. In other words, if T transforms a local frame F to frame F’, this method converts this SE3Trajectory from coordinates in F to coordinates in F’

to_se3(milestone)[source]

Converts a state parameter vector to a klampt.se3 element

class klampt.model.trajectory.SO3Trajectory(times=None, milestones=None)[source]

Bases: klampt.model.trajectory.GeodesicTrajectory

A trajectory that performs interpolation in SO3. Each milestone is a 9-D klampt.so3 element.

constructor()[source]
postTransform(R)[source]

Postmultiplies every rotation in here by the se3 element R. In other words, if R rotates a local frame F to frame F’, this method converts this SO3Trajectory from describing how F’ rotates to how F rotates.

preTransform(R)[source]

Premultiplies every rotation in here by the so3 element R. In other words, if R rotates a local frame F to frame F’, this method converts this SO3Trajectory from coordinates in F to coordinates in F’

class klampt.model.trajectory.Trajectory(times=None, milestones=None)[source]

A basic piecewise-linear trajectory class, which can be overloaded to provide different functionality. A plain Trajectory interpolates in Cartesian space.

(To interpolate for a robot, use RobotTrajectory. To perform Hermite interpolation, use HermiteTrajectory)

times

a list of times at which the milestones are met.

Type:list of floats
milestones

a list of milestones that are interpolated.

Type:list of Configs
Parameters:
  • times (list of floats, optional) – if provided, initializes the self.times attribute. If milestones is provided, a uniform timing is set. Otherwise self.times is empty.
  • milestones (list of Configs, optional) – if provided, initializes the self.milestones attribute. Otherwise milestones is empty.

Does not perform error checking. The caller must be sure that the lists have the same size, the times are non-decreasing, and the configs are equally-sized (you can call checkValid() for this).

after(time)[source]

Returns the part of the trajectory after the given time

before(time)[source]

Returns the part of the trajectory before the given time

checkValid()[source]

Checks whether this is a valid trajectory, raises a ValueError if not.

concat(suffix, relative=False, jumpPolicy='strict')[source]

Returns a new trajectory with another trajectory concatenated onto self.

Parameters:
  • suffix (Trajectory) – the suffix trajectory
  • relative (bool) – If True, then the suffix’s time domain is shifted so that self.times[-1] is added on before concatenation.
  • jumpPolicy (str) –

    If the suffix starts exactly at the existing trajectory’s end time, then jumpPolicy is checked. Can be:

    • ’strict’: the suffix’s first milestone has to be equal to the existing trajectory’s last milestone. Otherwise an exception is raised.
    • ’blend’: the existing trajectory’s last milestone is discarded.
    • ’jump’: a discontinuity is added to the trajectory.
constructor()[source]

Returns a “standard” constructor for the split / concat routines. The result should be a function that takes two arguments: a list of times and a list of milestones.

deriv(t, endBehavior='halt')[source]

Evaluates the trajectory velocity using piecewise linear interpolation.

Parameters:
  • t (float) – The time at which to evaluate the segment
  • endBehavior (str) – If ‘loop’ then the trajectory loops forever.
Returns:

the velocity (derivative) at time t

Return type:

(list of float)

difference(a, b, u, dt)[source]

Subclasses can override this to implement non-Cartesian spaces. Returns the time derivative along the geodesic from b to a. dt is the duration of the segment form a to b

discretize(dt)[source]

Returns a copy of this but with uniformly defined milestones at resolution dt. Start and goal are maintained exactly

duration()[source]

Returns the duration of the trajectory.

endTime()[source]

Returns the final time.

eval(t, endBehavior='halt')[source]

Evaluates the trajectory using piecewise linear interpolation.

Parameters:
  • t (float) – The time at which to evaluate the segment
  • endBehavior (str) – If ‘loop’ then the trajectory loops forever.
Returns:

the configuration at time t

Return type:

(list of float)

getSegment(t, endBehavior='halt')[source]

Returns the index and interpolation parameter for the segment at time t.

Running time is O(log n) time where n is the number of segments.

Parameters:
  • t (float) – The time at which to evaluate the segment
  • endBehavior (str) – If ‘loop’ then the trajectory loops forever.
Returns:

(index,param) giving the segment index and interpolation parameter

Return type:

(tuple)

insert(time)[source]

Inserts a milestone and keyframe at the given time. Returns the index of the new milestone, or if a milestone already exists, then it returns that milestone index.

If the path is empty, the milestone is set to an empty list [].

interpolate(a, b, u, dt)[source]

Can override this to implement non-cartesian spaces. Interpolates along the geodesic from a to b. dt is the duration of the segment from a to b

load(fn)[source]

Reads from a whitespace-separated file in the format:

t1 [q1]
t2 [q2]
...

where each [qi] is a Klamp’t formatted length-n configuration, written in the form n qi1 ... qin.

remesh(newtimes, tol=1e-06)[source]

Returns a path that has milestones at the times given in newtimes, as well as the current milestone times. Return value is (path,newtimeidx) where path is the remeshed path, and newtimeidx is a list of time indices for which path.times[newtimeidx[i]] = newtimes[i].

newtimes is an iterable over floats. It does not need to be sorted.

tol is a parameter specifying how closely the returned path must interpolate the original path. Old milestones will be dropped if they are not needed to follow the path within this tolerance.

The end behavior is assumed to be ‘halt’.

save(fn)[source]

Writes to a whitespace-separated file

splice(suffix, time=None, relative=False, jumpPolicy='strict')[source]

Returns a path such that the suffix is spliced in at some time

Parameters:
  • suffix (Trajectory) – the trajectory to splice in
  • time (float, optional) – determines when the splice occurs. The suffix is spliced in at the suffix’s start time if time=None, or the given time if specified.
  • jumpPolicy (str) – if ‘strict’, then it is required that suffix(t0)=path(t0) where t0 is the absolute start time of the suffix.
split(time)[source]

Returns a pair of trajectories obtained from splitting this one at the given time

startTime()[source]

Returns the initial time.

klampt.model.trajectory.execute_path(path, controller, speed=1.0, smoothing=None, activeDofs=None)[source]

Sends an untimed trajectory to a controller.

If smoothing = None, the path will be executed as a sequence of go-to commands, starting and stopping at each milestone. Otherwise, it will be smoothed somehow and sent to the controller as faithfully as possible.

Parameters:
  • path (list of Configs) – a list of milestones
  • controller (SimRobotController) – the controller to execute the path
  • speed (float, optional) – if given, changes the execution speed of the path. Not valid with smoothing=None or ‘ramp’.
  • smoothing (str, optional) –

    any smoothing applied to the path. Valid values are:

    • None: starts / stops at each milestone, moves in linear joint-space paths. Trapezoidal velocity profile used.
    • ’linear’: interpolates milestones linearly with fixed duration. Constant velocity profile used.
    • ’cubic’: interpolates milestones with cubic spline with fixed duration. Parabolic velocity profile used. Starts/stops at each milestone.
    • ’spline’: interpolates milestones smoothly with some differenced velocity.
    • ’ramp’: starts / stops at each milestone, moves in minimum-time / minimum- acceleration paths. Trapezoidal velocity profile used.
  • activeDofs (list, optional) – if not None, a list of dofs that are moved by the trajectory. Each entry may be an integer or a string.
klampt.model.trajectory.execute_trajectory(trajectory, controller, speed=1.0, smoothing=None, activeDofs=None)[source]

Sends a timed trajectory to a controller.

Parameters:
  • trajectory (Trajectory) – a Trajectory, RobotTrajectory, or HermiteTrajectory instance
  • controller (SimRobotController) – the controller to execute the trajectory
  • speed (float, optional) – modulates the speed of the path.
  • smoothing (str, optional) – any smoothing applied to the path. Only valid for piecewise linear trajectories. Valid values are * None: no smoothing, just do a piecewise linear trajectory * ‘spline’: interpolate tangents to the curve * ‘pause’: smoothly speed up and slow down
  • activeDofs (list, optional) – if not None, a list of dofs that are moved by the trajectory. Each entry may be an integer or a string.
klampt.model.trajectory.path_to_trajectory(path, velocities='auto', timing='limited', smoothing='spline', stoptol=None, vmax='auto', amax='auto', speed=1.0, dt=0.01, startvel=0.0, endvel=0.0, verbose=0)[source]

Converts an untimed path to a timed trajectory.

The resulting trajectory passes through each of the milestones without stopping, except for “stop” milestones. Stop milestones by default are only the first and last milestone, but if stoptol is given, then the trajectory will be stopped if the curvature of the path exceeds this value.

The first step is to assign each segment a ‘distance’ d[i] suggesting how much time it would take to traverse that much spatial distance. This distance assignment method is controlled by the timing parameter.

The second step is to smooth the spline, if smoothing=’spline’ is given (default).

The third step creates the trajectory, assigning the times and velocity profile as specified by the velocities parameter. velocities dictates how the overall velocity profile behaves from beginning to end, and basically, each profile gradually speeds up and slows down. The path length L = \(\sum_{i=1}^n d[i]\) determines the duration T of the trajectory, as follows:

  • For constant velocity profiles, T=L.
  • For trapezoidal, triangular, parabolic, and cosine, T = sqrt(L).
  • For minimum-jerk, T = L^(1/3).

The fourth step is to time scale the result to respect limits velocity / acceleration limits, if timing==’limited’ or speed==’limited’.

The fifth step is to time scale the result by speed.

Note

There are only some meaningful combinations of arguments:

  • velocities=’auto’; timing=’limited’: a generates a timed spline using a heuristic and then revises it to respect velocity and acceleration limits.

    The limited timing heuristic works best when the milestones are widely spaced.

    Be sure to specify vmax and amax if you don’t have a RobotTrajectory.

  • velocities=’auto’, trapezoidal’, ‘triangular’, ‘parabolic’, ‘cosine’, or ‘minimum-jerk’; timing=’L2’, ‘Linf’, ‘robot’, ‘sqrt-L2’, ‘sqrt-Linf’, or ‘sqrt-robot’: an entirely heuristic approach.

    The sqrt values lead to somewhat better tangents for smoothed splines with nonuniform distances between milestones.

    In these cases, vmax and amax are ignored.

  • If path uses non-Euclidean interpolation, then smoothing=None should be provided. Smoothing is not yet supported for non-Euclidean spaces (e.g., robots with special joints, SO3, SE3).

Parameters:
  • path – a list of milestones, a trajectory, or a RobotTrajectory. In the latter cases, if durations = ‘path’ then the durations are extracted from the trajectory’s timing.
  • velocities (str, optional) –

    the manner in which velocities are assigned along the path. Can be:

    • ’auto’ (default): if timing is ‘limited’, this is equivalent to ‘constant’. Otherwise, this is equivalent to ‘trapezoidal’.
    • ’trapezoidal’: a trapezoidal velocity profile with max acceleration and velocity. If timing is ‘limited’, the velocity max is determined by vmax. Otherwise, the ramp up proceeds for 1/4 of the time, stays constant 1/2 of the time, and then ramps down for 1/4 of the time.
    • ’constant’: the path is executed at fixed constant velocity
    • ’triangular’: velocities are ramped up for 1/2 of the duration and then ramped back down.
    • ’parabolic’: a parabolic curve (output is a Hermite spline)
    • ’cosine’: velocities follow (1-cosine)/2
    • ’minimum-jerk’: minimum jerk velocities
    • ’optimal’: uses time scaling optimization. NOT IMPLEMENTED YET
  • timing (optional) –

    affects how path timing between milestones is normalized. Valid options are:

    • ’limited’ (default): uses the vmax, amax variables along with the velocity profile to dynamically determine the duration assigned to each segment.
    • ’uniform’: base timing between milestones is uniform (1/(|path|*speed))
    • ’path’: only valid if path is a Trajectory object. Uses the timing in path.times as the base timing.
    • ’L2’: base timing is set proportional to L2 distance between milestones
    • ’Linf’: base timing is set proportional to L-infinity distance between milestones
    • ’robot’: base timing is set proportional to robot’s distance function between milestones
    • ’sqrt-L2’, ‘sqrt-Linf’, or ‘sqrt-robot’: base timing is set proportional to the square root of the L2, Linf, or robot distance between milestones
    • a list or tuple: the base timing is given in this list
    • callable function f(a,b): sets the normalization to the function f(a,b).
  • smoothing (str, optional) – if ‘spline’, the geometric path is first smoothed before assigning times. Otherwise, the geometric path is interpreted as a piecewise linear path.
  • stoptol (float, optional) – determines how start/stop segments are determined. If None, the trajectory only pauses at the start and end of the path. If 0, it pauses at every milestone. Otherwise, it pauses if the curvature at the milestone exceeds stoptol.
  • vmax (optional) –

    only meaningful if timing==’limited’. Can be:

    • ’auto’ (default): either 1 or the robot’s joint velocity limits if a RobotTrajectory is provided
    • a positive number: the L2 norm of the derivative of the result trajectory is limited to this value
    • a list of positive floats: the element-wise derivative of the result trajectory is limited to this value
  • amax (optional) –

    only meaningful if timing==’limited’. Can be:

    • ’auto’ (default): either 4 or the robot’s joint acceleration limits if a RobotTrajectory is provided
    • a positive number: the L2 norm of the acceleration of the result trajectory is limited to this value
    • a list of positive floats: the element-wise acceleration of the result trajectory is limited to this value.
  • speed (float or str, optional) – if a float, this is a speed multiplier applied to the resulting trajectory. This can also be ‘limited’, which applies the velocity and acceleration limits.
  • dt (float, optional) – the resolution of the resulting trajectory. Default 0.01.
  • startvel (float, optional) –

    the starting velocity of the path, given as a multiplier of path[1]-path[0]. Must be nonnegative.

    Note: might not be respected for some velocity profiles.

    Warning

    NOT IMPLEMENTED YET

  • endvel (float, optional) –

    the ending velocity of the path, given as a multiplier of path[-1]-path[-2]. Must be nonnegative.

    Note: might not be respected for some velocity profiles.

    Warning

    NOT IMPLEMENTED YET

  • verbose (int, optional) – if > 0, some debug printouts will be given.
Returns:

a finely-discretized, timed trajectory that is C1 continuous

and respects the limits defined in the arguments.

Return type:

(Trajectory)