klampt.model.trajectory module¶
Classes for loading, saving, evaluating, and operating on trajectories.
For piecewiselinear interpolation in cartesian space, use
Trajectory
.For piecewiselinear interpolation on a robot, use
RobotTrajectory
.For Hermite interpolation in cartesian space, use
HermiteTrajectory
.
Classes:

A basic piecewiselinear trajectory class, which can be overloaded to provide different functionality. 

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

A trajectory that performs interpolation on a GeodesicSpace. 

A trajectory that performs interpolation in SO3. 

A trajectory that performs interpolation in SE3. 

A trajectory that performs cubic interpolation between prescribed segment endpoints and velocities. 

A trajectory that performs Hermite interpolation on a GeodesicSpace using the DeCastlejau algorithm. 

A trajectory that performs Hermite interpolation in SO3. 

A trajectory that performs Bezier interpolation in SE3. 
Functions:

Converts an untimed path to a timed trajectory. 

Sends an untimed trajectory to a controller. 

Sends a timed trajectory to a controller. 

class
klampt.model.trajectory.
Trajectory
(times=None, milestones=None)[source]¶ Bases:
object
A basic piecewiselinear 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 nondecreasing, and the configs are equallysized (you can call checkValid() for this).
Methods:
load
(fn)Reads from a whitespaceseparated file in the format.
save
(fn)Writes to a whitespaceseparated file
Returns the initial time.
endTime
()Returns the final time.
duration
()Returns the duration of the trajectory.
Checks whether this is a valid trajectory, raises a ValueError if not.
getSegment
(t[, endBehavior])Returns the index and interpolation parameter for the segment at time t.
eval
(t[, endBehavior])Evaluates the trajectory using piecewise linear interpolation.
deriv
(t[, endBehavior])Evaluates the trajectory velocity using piecewise linear interpolation.
waypoint
(state)Returns the primary configuration corresponding to the given state.
eval_state
(t[, endBehavior])Internal eval, used on the underlying state representation
deriv_state
(t[, endBehavior])Internal deriv, used on the underlying state representation
interpolate_state
(a, b, u, dt)Can override this to implement noncartesian spaces.
difference_state
(a, b, u, dt)Subclasses can override this to implement nonCartesian spaces.
concat
(suffix[, relative, jumpPolicy])Returns a new trajectory with another trajectory concatenated onto self.
insert
(time)Inserts a milestone and keyframe at the given time.
split
(time)Returns a pair of trajectories obtained from splitting this one at the given time.
before
(time)Returns the part of the trajectory before the given time
after
(time)Returns the part of the trajectory after the given time
splice
(suffix[, time, relative, jumpPolicy])Returns a path such that the suffix is spliced in at some time
Returns a “standard” constructor for the split / concat routines.
length
([metric])Returns the arclength of the trajectory, according to the given metric.
discretize_state
(dt)Returns a copy of this but with uniformly defined milestones at resolution dt.
discretize
(dt)Returns a trajectory, uniformly discretized at resolution dt, and with statespace the same as its configuration space.
remesh
(newtimes[, tol])Returns a path that has milestones at the times given in newtimes, as well as the current milestone times.
extractDofs
(dofs)Extracts a trajectory just over the given DOFs.
stackDofs
(trajs[, strict])Stacks the degrees of freedom of multiple trajectories together.

load
(fn)[source]¶ Reads from a whitespaceseparated file in the format:
t1 [q1] t2 [q2] ...
where each [qi] is a Klamp’t formatted lengthn configuration, written in the form
n qi1 ... qin
. Return type
None

checkValid
()[source]¶ Checks whether this is a valid trajectory, raises a ValueError if not.
 Return type
None

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. index < 0 indicates that the time is before the first milestone and/or there is only 1 milestone.
 Return type
Tuple
[int
,float
]

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
Sequence
[float
]

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
Sequence
[float
]

waypoint
(state)[source]¶ Returns the primary configuration corresponding to the given state.
This is usually the same as
state
but for some trajectories, specifically Hermite curves, the state and configuration are not identically the same. Return type
Sequence
[float
]

eval_state
(t, endBehavior='halt')[source]¶ Internal eval, used on the underlying state representation
 Return type
Sequence
[float
]

deriv_state
(t, endBehavior='halt')[source]¶ Internal deriv, used on the underlying state representation
 Return type
Sequence
[float
]

interpolate_state
(a, b, u, dt)[source]¶ Can override this to implement noncartesian spaces. Interpolates along the geodesic from a to b. dt is the duration of the segment from a to b
 Return type
Sequence
[float
]

difference_state
(a, b, u, dt)[source]¶ Subclasses can override this to implement nonCartesian spaces. Returns the time derivative along the geodesic from b to a, with time domain [0,dt]. In cartesian spaces, this is (ab)/dt.
 Parameters
a (vector) – the end point of the segment
b (vector) – the start point of the segment.
u (float) – the evaluation point of the derivative along the segment, with 0 indicating b and 1 indicating a
dt (float) – the duration of the segment from b to a.
 Return type
Sequence
[float
]

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.
 Return type

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 [].
 Return type
int

split
(time)[source]¶ Returns a pair of trajectories obtained from splitting this one at the given time.
 Returns
A pair (prefix,suffix) satisfying prefix.endTime()==time, suffix.startTime()==time, and prefix.milestones[1]==suffix.milestones[0]
 Return type
Tuple
[Trajectory
,Trajectory
]

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.
 Return type

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.
 Return type
Callable
[[List
,List
],Trajectory
]

length
(metric=None)[source]¶ Returns the arclength of the trajectory, according to the given metric.
If metric = None, uses the “natural” metric for this trajectory, which is usually Euclidean. Otherwise it is a function f(a,b) from configurations to nonnegative numbers.
 Return type
float

discretize_state
(dt)[source]¶ Returns a copy of this but with uniformly defined milestones at resolution dt. Start and goal are maintained exactly
 Return type

discretize
(dt)[source]¶ Returns a trajectory, uniformly discretized at resolution dt, and with statespace the same as its configuration space. Similar to discretize, but if the state space is of higher dimension (e.g., Hermite trajectories) this projects to a piecewise linear trajectory.
 Return type

remesh
(newtimes, tol=1e06)[source]¶ Returns a path that has milestones at the times given in newtimes, as well as the current milestone times.
 Parameters
newtimes – an iterable over floats. It does not need to be sorted.
tol (float, optional) – 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’.
 Returns
A tuple (path,newtimeidx) where path is the remeshed path, and newtimeidx is a list of time indices satisfying
path.times[newtimeidx[i]] = newtimes[i]
. Return type
Tuple
[Trajectory
,List
[int
]]

extractDofs
(dofs)[source]¶ Extracts a trajectory just over the given DOFs.
 Parameters
dofs (list of int) – the indices to extract.
 Returns
A copy of this trajectory but only over the given DOFs.
 Return type

stackDofs
(trajs, strict=True)[source]¶ Stacks the degrees of freedom of multiple trajectories together. The result is contained in self.
All evaluations are assumed to take place with the ‘halt’ endBehavior.
 Parameters
trajs (list or tuple of Trajectory) – the trajectories to stack
strict (bool, optional) – if True, will warn if the classes of the trajectories do not match self.
 Return type
None


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.
 Parameters
robot (RobotModel or SubRobotModel) – the robot whose configuration should follow this trajectory.
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.
Methods:
interpolate_state
(a, b, u, dt)Can override this to implement noncartesian spaces.
difference_state
(a, b, u, dt)Subclasses can override this to implement nonCartesian spaces.
Returns a “standard” constructor for the split / concat routines.
getLinkTrajectory
(link[, discretization])Computes the link’s trajectory as the robot follows this trajectory.
length
([metric])Returns the arclength of the trajectory, according to the given metric.
Checks whether this is a valid trajectory, raises a ValueError if not.
extractDofs
(dofs)Extracts a RobotTrajectory just over the given DOFs.
stackDofs
(trajs)Stacks the degrees of freedom of multiple trajectories together.

interpolate_state
(a, b, u, dt)[source]¶ Can override this to implement noncartesian spaces. Interpolates along the geodesic from a to b. dt is the duration of the segment from a to b

difference_state
(a, b, u, dt)[source]¶ Subclasses can override this to implement nonCartesian spaces. Returns the time derivative along the geodesic from b to a, with time domain [0,dt]. In cartesian spaces, this is (ab)/dt.
 Parameters
a (vector) – the end point of the segment
b (vector) – the start point of the segment.
u (float) – the evaluation point of the derivative along the segment, with 0 indicating b and 1 indicating a
dt (float) – the duration of the segment from b to a.

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.

getLinkTrajectory
(link, discretization=None)[source]¶ Computes the link’s trajectory as the robot follows this trajectory. If discretization = None, only the milestones are extracted. Otherwise, the piecewise linear approximation at dt = discretization is used.
 Returns
The SE3Trajectory for the link’s origin approximating its path as the robot executes this trajectory.
 Return type

length
(metric=None)[source]¶ Returns the arclength of the trajectory, according to the given metric.
If metric = None, uses the “natural” metric for this trajectory, which is usually Euclidean. Otherwise it is a function f(a,b) from configurations to nonnegative numbers.

extractDofs
(dofs)[source]¶ Extracts a RobotTrajectory just over the given DOFs.
 Parameters
dofs (list of int or str) – the indices to extract
 Returns
A copy of this trajectory but over a SubRobotModel.
 Return type

stackDofs
(trajs)[source]¶ Stacks the degrees of freedom of multiple trajectories together. The result is contained in self.
All evaluations are assumed to take place with the ‘halt’ endBehavior.
 Parameters
trajs (list or tuple of Trajectory) – the trajectories to stack
strict (bool, optional) – if True, will warn if the classes of the trajectories do not match self.

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.math.geodesic
for more information. 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 nondecreasing, and the configs are equallysized (you can call checkValid() for this).
Methods:
interpolate_state
(a, b, u, dt)Can override this to implement noncartesian spaces.
difference_state
(a, b, u, dt)Subclasses can override this to implement nonCartesian spaces.
Returns a “standard” constructor for the split / concat routines.
length
([metric])Returns the arclength of the trajectory, according to the given metric.
Checks whether this is a valid trajectory, raises a ValueError if not.
extractDofs
(dofs)Invalid for GeodesicTrajectory.
stackDofs
(trajs)Stacks the degrees of freedom of multiple trajectories together.

interpolate_state
(a, b, u, dt)[source]¶ Can override this to implement noncartesian spaces. Interpolates along the geodesic from a to b. dt is the duration of the segment from a to b

difference_state
(a, b, u, dt)[source]¶ Subclasses can override this to implement nonCartesian spaces. Returns the time derivative along the geodesic from b to a, with time domain [0,dt]. In cartesian spaces, this is (ab)/dt.
 Parameters
a (vector) – the end point of the segment
b (vector) – the start point of the segment.
u (float) – the evaluation point of the derivative along the segment, with 0 indicating b and 1 indicating a
dt (float) – the duration of the segment from b to a.

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.

length
(metric=None)[source]¶ Returns the arclength of the trajectory, according to the given metric.
If metric = None, uses the “natural” metric for this trajectory, which is usually Euclidean. Otherwise it is a function f(a,b) from configurations to nonnegative numbers.

stackDofs
(trajs)[source]¶ Stacks the degrees of freedom of multiple trajectories together. The result is contained in self.
All evaluations are assumed to take place with the ‘halt’ endBehavior.
 Parameters
trajs (list or tuple of Trajectory) – the trajectories to stack
strict (bool, optional) – if True, will warn if the classes of the trajectories do not match self.

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 9D
klampt.math.so3
element. 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 nondecreasing, and the configs are equallysized (you can call checkValid() for this).
Methods:
deriv_angvel
(t[, endBehavior])Returns the derivative at t, in angular velocity form.
preTransform
(R)Premultiplies every rotation in here by the so3 element R.
Postmultiplies every rotation in here by the se3 element R.
getPointTrajectory
(localPt)Returns a Trajectory approximating the movement of a point localPt attached to this rotating frame.
Checks whether this is a valid trajectory, raises a ValueError if not.
Returns a “standard” constructor for the split / concat routines.

deriv_angvel
(t, endBehavior='halt')[source]¶ Returns the derivative at t, in angular velocity form.
 Return type
Sequence
[float
]

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’.
 Return type
None

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.
 Return type
None

class
klampt.model.trajectory.
SE3Trajectory
(times=None, milestones=None)[source]¶ Bases:
klampt.model.trajectory.GeodesicTrajectory
A trajectory that performs interpolation in SE3. Each milestone (state) is a 12D flattened
klampt.math.se3
element (i.e., the concatenation of R + t for an (R,t) pair).Constructor can take either a list of SE3 elements or 12element vectors.
 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 nondecreasing, and the configs are equallysized (you can call checkValid() for this).
Methods:
to_se3
(state)Converts a state parameter vector to a klampt.se3 element
waypoint
(state)Returns the primary configuration corresponding to the given state.
from_se3
(T)Converts a klampt.se3 element to a state parameter vector
eval
(t[, endBehavior])Returns an SE3 element
deriv
(t[, endBehavior])Returns the derivative as the derivatives of an SE3 element
deriv_screw
(t[, endBehavior])Returns the derivative at t, in screw form, that is, a 6D (angular velocity,velocity) vector.
preTransform
(T)Premultiplies every transform in self by the se3 element T.
Postmultiplies every transform in self by the se3 element T.
Returns an SO3Trajectory describing the rotation trajectory.
getPositionTrajectory
([localPt])Returns a Trajectory approximating the movement of the given local point localPt (or the origin, if none is provided).
Checks whether this is a valid trajectory, raises a ValueError if not.
extractDofs
(dofs)Invalid for GeodesicTrajectory.
Returns a “standard” constructor for the split / concat routines.

to_se3
(state)[source]¶ Converts a state parameter vector to a klampt.se3 element
 Return type
Tuple
[Sequence
[float
],Sequence
[float
]]

waypoint
(state)[source]¶ Returns the primary configuration corresponding to the given state.
This is usually the same as
state
but for some trajectories, specifically Hermite curves, the state and configuration are not identically the same. Return type
Tuple
[Sequence
[float
],Sequence
[float
]]

from_se3
(T)[source]¶ Converts a klampt.se3 element to a state parameter vector
 Return type
Sequence
[float
]

eval
(t, endBehavior='halt')[source]¶ Returns an SE3 element
 Return type
Tuple
[Sequence
[float
],Sequence
[float
]]

deriv
(t, endBehavior='halt')[source]¶ Returns the derivative as the derivatives of an SE3 element
 Return type
Tuple
[Sequence
[float
],Sequence
[float
]]

deriv_screw
(t, endBehavior='halt')[source]¶ Returns the derivative at t, in screw form, that is, a 6D (angular velocity,velocity) vector.
 Return type
Tuple
[Sequence
[float
],Sequence
[float
]]

preTransform
(T)[source]¶ Premultiplies every transform in self 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’
 Return type
None

postTransform
(T)[source]¶ Postmultiplies every transform in self 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.
 Return type
None

getRotationTrajectory
()[source]¶ Returns an SO3Trajectory describing the rotation trajectory.
 Return type

getPositionTrajectory
(localPt=None)[source]¶ Returns a Trajectory approximating the movement of the given local point localPt (or the origin, if none is provided).
To improve the accuracy of this approximation, first discretize this trajectory at a finer discretization.
 Return type

class
klampt.model.trajectory.
HermiteTrajectory
(times=None, milestones=None, dmilestones=None)[source]¶ Bases:
klampt.model.trajectory.Trajectory
A trajectory that performs cubic interpolation between prescribed segment endpoints and velocities.
The milestones (states) are given in phase space (x,dx).
eval(t)
returns the primary configuration x, andderiv(t)
returns the velocity dx. To get acceleration, useaccel(t)
. To get the state space (x,dx), useeval_state(t)
. Parameters
times (list of float, optional) – the knot points
milestones (list of lists, optional) – the milestones met at the knot points.
dmilestones (list of lists, optional) – the velocities (derivatives w.r.t time) at each knot point.
Possible constructor options are:
HermiteTrajectory(): empty trajectory
HermiteTrajectory(times,milestones): milestones contains 2ND lists consisting of the concatenation of a point and its outgoing velocity.
HermiteTrajectory(times,milestones,dmilestones): milestones and dmilestones each contain ND lists defining the points and outgoing velocities.
Note: the curve is assumed to be smooth. To make a nonsmooth curve, duplicate the knot point and milestone, but set a different velocity at the copy.
 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 nondecreasing, and the configs are equallysized (you can call checkValid() for this).
Methods:
makeSpline
(waypointTrajectory[, …])Computes natural velocities for a standard configuration space Trajectory to make it smoother.
makeBezier
(times, controlPoints)Sets up this spline to perform Bezier interpolation of the given control points, with segment 0 a Bezier curve on cps[0:3], segment 1 a Bezier curve on cps[3:6], etc.
makeMinTimeSpline
(milestones[, velocities, …])Creates a spline that interpolates between the given milestones with bounded velocities, accelerations, and positions.
waypoint
(state)Returns the primary configuration corresponding to the given state.
eval_state
(t[, endBehavior])Returns the (configuration,velocity) state at time t.
eval
(t[, endBehavior])Returns just the configuration component of the result
deriv
(t[, endBehavior])Returns just the velocity component of the result
eval_accel
(t[, endBehavior])Returns just the acceleration component of the derivative
interpolate_state
(a, b, u, dt)Can override this to implement noncartesian spaces.
difference_state
(a, b, u, dt)Subclasses can override this to implement nonCartesian spaces.
discretize
(dt)Creates a discretized piecewise linear Trajectory in config space that approximates this curve with resolution dt.
length
()Returns an upper bound on length given by the Bezier property.
Checks whether this is a valid trajectory, raises a ValueError if not.
extractDofs
(dofs)Extracts a trajectory just over the given DOFs.
stackDofs
(trajs[, strict])Stacks the degrees of freedom of multiple trajectories together.
Returns a “standard” constructor for the split / concat routines.

makeSpline
(waypointTrajectory, preventOvershoot=True, loop=False)[source]¶ Computes natural velocities for a standard configuration space Trajectory to make it smoother.
 Return type
None

makeBezier
(times, controlPoints)[source]¶ Sets up this spline to perform Bezier interpolation of the given control points, with segment 0 a Bezier curve on cps[0:3], segment 1 a Bezier curve on cps[3:6], etc.
 Return type
None

makeMinTimeSpline
(milestones, velocities=None, xmin=None, xmax=None, vmax=None, amax=None)[source]¶ Creates a spline that interpolates between the given milestones with bounded velocities, accelerations, and positions.
If velocities==None, this requires the spline to move in a straight configurationspace path between the given milestones. This option is helpful for postprocessing the results for kinematic motion planning, for example.
 Return type
None

waypoint
(state)[source]¶ Returns the primary configuration corresponding to the given state.
This is usually the same as
state
but for some trajectories, specifically Hermite curves, the state and configuration are not identically the same.

eval_accel
(t, endBehavior='halt')[source]¶ Returns just the acceleration component of the derivative
 Return type
Sequence
[float
]

interpolate_state
(a, b, u, dt)[source]¶ Can override this to implement noncartesian spaces. Interpolates along the geodesic from a to b. dt is the duration of the segment from a to b

difference_state
(a, b, u, dt)[source]¶ Subclasses can override this to implement nonCartesian spaces. Returns the time derivative along the geodesic from b to a, with time domain [0,dt]. In cartesian spaces, this is (ab)/dt.
 Parameters
a (vector) – the end point of the segment
b (vector) – the start point of the segment.
u (float) – the evaluation point of the derivative along the segment, with 0 indicating b and 1 indicating a
dt (float) – the duration of the segment from b to a.

discretize
(dt)[source]¶ Creates a discretized piecewise linear Trajectory in config space that approximates this curve with resolution dt.

length
()[source]¶ Returns an upper bound on length given by the Bezier property. Faster than calculating the true length. To retrieve an approximation of true length, use self.discretize(dt).length().
 Return type
float

extractDofs
(dofs)[source]¶ Extracts a trajectory just over the given DOFs.
 Parameters
dofs (list of int) – the (primary) indices to extract. Each entry
be < len (must) –
 Returns
A copy of this trajectory but only over the given DOFs.
 Return type

stackDofs
(trajs, strict=True)[source]¶ Stacks the degrees of freedom of multiple trajectories together. The result is contained in self.
All evaluations are assumed to take place with the ‘halt’ endBehavior.
 Parameters
trajs (list or tuple of HermiteTrajectory) – the trajectories to stack
strict (bool, optional) – ignored. Will always warn for invalid classes.
 Return type
None

class
klampt.model.trajectory.
GeodesicHermiteTrajectory
(geodesic, times=None, milestones=None, outgoingLieDerivatives=None)[source]¶ Bases:
klampt.model.trajectory.Trajectory
A trajectory that performs Hermite interpolation on a GeodesicSpace using the DeCastlejau algorithm.
The milestones are a concatenation of the segment start point and the outgoing Lie derivatives w.r.t. t. The incoming Lie derivative at the segment end point is assumed to be the negative of the outgoing Lie derivative.
 Parameters
geodesic (GeodesicSpace) – the underlying space
times (list of floats, optional) – the knot points defining each segment
milestones (list of lists, optional) – the points at the ends of each segment
outgoingLieDerivatives (list of lists, optional) – the Lie derivatives (velocities) at the ends of each segment.
Possible constructor options are:
GeodesicHermiteTrajectory(geodesic): empty trajectory
GeodesicHermiteTrajectory(geodesic,times,milestones): milestones contains 2ND lists consisting of the concatenation of a point and its outgoing Lie derivative.
GeodesicHermiteTrajectory(geodesic,times,milestones,lieDerivatives): milestones and lieDerivatives contain ND lists defining the points and outgoing Lie derivatives.
Note: the curve is assumed to be smooth. To make a nonsmooth curve, duplicate the knot point and milestone, but set a different Lie derivative at the copy.
 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 nondecreasing, and the configs are equallysized (you can call checkValid() for this).
Methods:
makeSpline
(waypointTrajectory[, loop])Creates a spline from a set of waypoints, with smooth interpolation between waypoints.
makeBezier
(times, controlPoints)Sets up this spline to perform Bezier interpolation of the given control points, with segment 0 a Bezier curve on cps[0:3], segment 1 a Bezier curve on cps[3:6], etc.
waypoint
(state)Returns the primary configuration corresponding to the given state.
interpolate_state
(a, b, u, dt)Can override this to implement noncartesian spaces.
difference_state
(a, b, u, dt)Subclasses can override this to implement nonCartesian spaces.
eval_state
(t[, endBehavior])Returns the (configuration,velocity) state at time t.
eval
(t[, endBehavior])Evaluates the configuration at time t
deriv
(t[, endBehavior])Returns the velocity at time t
discretize
(dt)Creates a discretized piecewisegeodesic (GeodesicTrajectory) approximation of this curve in config space, with resolution dt.
length
()Returns an upper bound on length given by the Bezier property.
Checks whether this is a valid trajectory, raises a ValueError if not.
extractDofs
(dofs)Invalid for GeodesicHermiteTrajectory.
stackDofs
(trajs[, strict])Stacks the degrees of freedom of multiple trajectories together.
Returns a “standard” constructor for the split / concat routines.

makeSpline
(waypointTrajectory, loop=False)[source]¶ Creates a spline from a set of waypoints, with smooth interpolation between waypoints.
 Return type
None

makeBezier
(times, controlPoints)[source]¶ Sets up this spline to perform Bezier interpolation of the given control points, with segment 0 a Bezier curve on cps[0:3], segment 1 a Bezier curve on cps[3:6], etc.
 Return type
None

waypoint
(state)[source]¶ Returns the primary configuration corresponding to the given state.
This is usually the same as
state
but for some trajectories, specifically Hermite curves, the state and configuration are not identically the same.

interpolate_state
(a, b, u, dt)[source]¶ Can override this to implement noncartesian spaces. Interpolates along the geodesic from a to b. dt is the duration of the segment from a to b

difference_state
(a, b, u, dt)[source]¶ Subclasses can override this to implement nonCartesian spaces. Returns the time derivative along the geodesic from b to a, with time domain [0,dt]. In cartesian spaces, this is (ab)/dt.
 Parameters
a (vector) – the end point of the segment
b (vector) – the start point of the segment.
u (float) – the evaluation point of the derivative along the segment, with 0 indicating b and 1 indicating a
dt (float) – the duration of the segment from b to a.

discretize
(dt)[source]¶ Creates a discretized piecewisegeodesic (GeodesicTrajectory) approximation of this curve in config space, with resolution dt.

length
()[source]¶ Returns an upper bound on length given by the Bezier property. Faster than calculating the true length. To retrieve an approximation of true length, use self.discretize(dt).length().
 Return type
float

stackDofs
(trajs, strict=True)[source]¶ Stacks the degrees of freedom of multiple trajectories together. The result is contained in self.
All evaluations are assumed to take place with the ‘halt’ endBehavior.
 Parameters
trajs (list or tuple of Trajectory) – the trajectories to stack
strict (bool, optional) – if True, will warn if the classes of the trajectories do not match self.

class
klampt.model.trajectory.
SO3HermiteTrajectory
(times=None, milestones=None, outgoingLieDerivatives=None)[source]¶ Bases:
klampt.model.trajectory.GeodesicHermiteTrajectory
A trajectory that performs Hermite interpolation in SO3. Each milestone is 18D, consisting of a 9D
klampt.math.so3
element and its subsequent Lie derivative. Parameters
times (list of floats, optional) – knot points.
milestones (list of Configs, optional) – list of waypoint orientations.
outgoingLieDerivatives (list of 9D lists) – list of Lie derivatives, i.e. cross product matrix (
cross_product()
) for each angular velocity.times – if provided, initializes the self.times attribute. If milestones is provided, a uniform timing is set. Otherwise self.times is empty.
milestones – 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 nondecreasing, and the configs are equallysized (you can call checkValid() for this).
Methods:
preTransform
(R)Premultiplies every rotation in here by the so3 element R.
deriv_angvel
(t[, endBehavior])Returns the derivative at t, in angular velocity form
Postmultiplies every rotation in here by the se3 element R.
discretize
(dt)Creates a discretized piecewisegeodesic (GeodesicTrajectory) approximation of this curve in config space, with resolution dt.
Returns a “standard” constructor for the split / concat routines.

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 SO3HermiteTrajectory from coordinates in F to coordinates in F’
 Return type
None

deriv_angvel
(t, endBehavior='halt')[source]¶ Returns the derivative at t, in angular velocity form
 Return type
Sequence
[float
]

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 SO3HermiteTrajectory from describing how F’ rotates to how F rotates.
 Return type
None

class
klampt.model.trajectory.
SE3HermiteTrajectory
(times=None, milestones=None, outgoingLieDerivatives=None)[source]¶ Bases:
klampt.model.trajectory.GeodesicHermiteTrajectory
A trajectory that performs Bezier interpolation in SE3. Each milestone is 24D, consisting of a 12D flattened
klampt.math.se3
element and its subsequent Lie derivative. 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 nondecreasing, and the configs are equallysized (you can call checkValid() for this).
Methods:
to_se3
(state)Converts a state parameter vector to a klampt.se3 element
from_se3
(T)Converts a klampt.se3 element to a state parameter vector
waypoint
(state)Returns the primary configuration corresponding to the given state.
eval
(t[, endBehavior])Returns an SE3 element
deriv
(t[, endBehavior])Returns the derivative as the derivatives of an SE3 element
deriv_screw
(t[, endBehavior])Returns the derivative at t, in screw vector form, that is, a 6D vector (angular velocity, velocity).
preTransform
(T)Premultiplies every transform in here by the se3 element T.
Postmultiplies every transform in here by the se3 element R.
discretize
(dt)Creates a discretized piecewisegeodesic (GeodesicTrajectory) approximation of this curve in config space, with resolution dt.
Returns a “standard” constructor for the split / concat routines.

to_se3
(state)[source]¶ Converts a state parameter vector to a klampt.se3 element
 Return type
Tuple
[Sequence
[float
],Sequence
[float
]]

from_se3
(T)[source]¶ Converts a klampt.se3 element to a state parameter vector
 Return type
Sequence
[float
]

waypoint
(state)[source]¶ Returns the primary configuration corresponding to the given state.
This is usually the same as
state
but for some trajectories, specifically Hermite curves, the state and configuration are not identically the same.

eval
(t, endBehavior='halt')[source]¶ Returns an SE3 element
 Return type
Tuple
[Sequence
[float
],Sequence
[float
]]

deriv
(t, endBehavior='halt')[source]¶ Returns the derivative as the derivatives of an SE3 element
 Return type
Tuple
[Sequence
[float
],Sequence
[float
]]

deriv_screw
(t, endBehavior='halt')[source]¶ Returns the derivative at t, in screw vector form, that is, a 6D vector (angular velocity, velocity).
 Return type
Sequence
[float
]

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 SE3HermiteTrajectory from coordinates in F to coordinates in F’
 Return type
None

postTransform
(T)[source]¶ Postmultiplies every transform 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.
 Return type
None

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 minimumjerk, 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 ‘minimumjerk’; timing=’L2’, ‘Linf’, ‘robot’, ‘sqrtL2’, ‘sqrtLinf’, or ‘sqrtrobot’: 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 nonEuclidean interpolation, then smoothing=None should be provided. Smoothing is not yet supported for nonEuclidean 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 (1cosine)/2
’minimumjerk’: 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 Linfinity distance between milestones
’robot’: base timing is set proportional to robot’s distance function between milestones
’sqrtL2’, ‘sqrtLinf’, or ‘sqrtrobot’: 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 elementwise 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 elementwise 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 finelydiscretized, timed trajectory that is C1 continuous and respects the limits defined in the arguments.
 Return type

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 goto 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 or RobotInterfaceBase) – 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 jointspace paths. Trapezoidal velocity profile used. This is most useful for executing paths coming from a kinematic motion planner.
’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 minimumtime / minimumacceleration 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 or RobotInterfaceBase) – 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.