klampt.math.autodiff.trajectory_ad module¶
Klampt trajectory AD functions:
Function
Derivative
Notes
hermite
Y
hermite(x1,v1,x2,v2,u) -> R^n
GeodesicInterpolate
1(u)
ginterp[geodesic](a,b,u) -> R^n
GeodesicDistance
1
gdist[geodesic](a,b) -> R
TrajectoryEval
1
eval[Trajectory,endBehavior](t) -> R^n
Trajectory
1
trajectory[n,end](ts,xs,t) -> R^n
Module summary¶
Autodiff’ed version of the |
|
|
Autodiff wrapper of geodesic.interpolate(a,b,u). |
|
Autodiff wrapper of geodesic.distance(a,b). |
|
Autodiff wrapper of traj.eval(t). |
|
Autodiff piecewise linear interpolation traj(times,milestone_stack,t). |
-
class
klampt.math.autodiff.trajectory_ad.
GeodesicDistance
(geodesic)[source]¶ Bases:
klampt.math.autodiff.ad.ADFunctionInterface
Autodiff wrapper of geodesic.distance(a,b).
-
eval
(a, b)[source]¶ Evaluates the application of the function to the given (instantiated) arguments.
- Parameters
args (list) – a list of arguments, which are either ndarrays or scalars.
-
jvp
(arg, darg, a, b)[source]¶ Performs a Jacobian-vector product for argument #arg.
- Parameters
arg (int) – A value from 0,…,self.n_args()-1 indicating that we wish to calculate df/dx_arg * darg.
darg (ndarray) – the derivative of x_arg w.r.t some other parameter. Must have
darg.shape = (self.n_in(arg),)
.args (list of ndarrays) – arguments for the function.
- Returns
A numpy array of length
self.n_out()
If the derivative is not implemented, raise a NotImplementedError.
- Return type
ndarray
-
-
class
klampt.math.autodiff.trajectory_ad.
GeodesicInterpolate
(geodesic)[source]¶ Bases:
klampt.math.autodiff.ad.ADFunctionInterface
Autodiff wrapper of geodesic.interpolate(a,b,u).
-
eval
(a, b, u)[source]¶ Evaluates the application of the function to the given (instantiated) arguments.
- Parameters
args (list) – a list of arguments, which are either ndarrays or scalars.
-
jvp
(arg, darg, a, b, u)[source]¶ Performs a Jacobian-vector product for argument #arg.
- Parameters
arg (int) – A value from 0,…,self.n_args()-1 indicating that we wish to calculate df/dx_arg * darg.
darg (ndarray) – the derivative of x_arg w.r.t some other parameter. Must have
darg.shape = (self.n_in(arg),)
.args (list of ndarrays) – arguments for the function.
- Returns
A numpy array of length
self.n_out()
If the derivative is not implemented, raise a NotImplementedError.
- Return type
ndarray
-
-
class
klampt.math.autodiff.trajectory_ad.
Trajectory
(n_or_robot, endBehavior='halt')[source]¶ Bases:
klampt.math.autodiff.ad.ADFunctionInterface
Autodiff piecewise linear interpolation traj(times,milestone_stack,t). The dimensionality or a robot must be provided on initialization.
milestone_stack
is a flattened array of the trajectory’s milestones.Also,
t
can be either a scalar or an array of times. Providing an array of times is faster than calling this repeatedly for multiple times.- Parameters
n_or_robot (int or RobotModel) – the dimension of the state, or the robot. In this case the milestones must be robot configurations.
endBehavior (string) – how to evaluate out-of-domain times. Can be ‘halt’ or ‘loop’.
-
eval
(ts, xs, t)[source]¶ Evaluates the application of the function to the given (instantiated) arguments.
- Parameters
args (list) – a list of arguments, which are either ndarrays or scalars.
-
jvp
(arg, darg, ts, xs, t)[source]¶ Performs a Jacobian-vector product for argument #arg.
- Parameters
arg (int) – A value from 0,…,self.n_args()-1 indicating that we wish to calculate df/dx_arg * darg.
darg (ndarray) – the derivative of x_arg w.r.t some other parameter. Must have
darg.shape = (self.n_in(arg),)
.args (list of ndarrays) – arguments for the function.
- Returns
A numpy array of length
self.n_out()
If the derivative is not implemented, raise a NotImplementedError.
- Return type
ndarray
-
class
klampt.math.autodiff.trajectory_ad.
TrajectoryEval
(traj, endBehavior='halt')[source]¶ Bases:
klampt.math.autodiff.ad.ADFunctionInterface
Autodiff wrapper of traj.eval(t).
- Parameters
traj (Trajectory) – the trajectory to evaluate. Can be any Trajectory subclass.
endBehavior (string) – how to evaluate out-of-domain times. Can be ‘halt’ or ‘loop’.
-
derivative
(arg, t)[source]¶ Returns the Jacobian of the function w.r.t. argument #arg.
- Parameters
arg (int) – A value from 0,…,self.n_args()-1 indicating that we wish to take \(df/dx_{arg}\).
args (list of ndarrays) – arguments for the function.
- Returns
A numpy array of shape
(self.n_out(),self.n_in(arg))
. Keep in mind that even if the argument or result is a scalar, this needs to be a 2D array.If the derivative is not implemented, raise a NotImplementedError.
If the derivative is zero, can just return 0 (the integer) regardless of the size of the result.
- Return type
ndarray
-
eval
(t)[source]¶ Evaluates the application of the function to the given (instantiated) arguments.
- Parameters
args (list) – a list of arguments, which are either ndarrays or scalars.
-
jvp
(arg, darg, t)[source]¶ Performs a Jacobian-vector product for argument #arg.
- Parameters
arg (int) – A value from 0,…,self.n_args()-1 indicating that we wish to calculate df/dx_arg * darg.
darg (ndarray) – the derivative of x_arg w.r.t some other parameter. Must have
darg.shape = (self.n_in(arg),)
.args (list of ndarrays) – arguments for the function.
- Returns
A numpy array of length
self.n_out()
If the derivative is not implemented, raise a NotImplementedError.
- Return type
ndarray
-
klampt.math.autodiff.trajectory_ad.
hermite
(*args) = <klampt.math.autodiff.ad._ad_trajectory.hermite object>¶ Autodiff’ed version of the
klampt.math.spline.hermite()
function. Takes Arguments (x1,v1,x2,v2,u) defining a Hermite curve with endpoints x1,x2 and velocities v1,v2, and returns the interpolant at parameter u in [0,1].