klampt.math.autodiff.kinematics_ad module

Klampt kinematics AD functions:

Function	Derivative	Notes
WorldPosition	1,2	wp[link,localPos](q) -> R^3
WorldDirection	1,2	wd[link,localDir](q) -> R^3
WorldOrientation	1,2	wo[link](q) -> SO(3)
WorldTransform	1,2	wt[link,*localPos](q) -> SE(3)
WorldVelocity	1	wv[link,localPos](q,dq) -> R^3
WorldAngularVelocity	1	ww[link](q,dq) -> R^3
#LocalPosition	1	lp[link,worldPos](q) -> R^3
#LocalDirection	1	ld[link,worldDir](q) -> R^3
#LocalOrientation	1	lo[link](q) -> SO(3)
DriversToLinks	Y	d2l[robot](qdriver) -> R^numLinks()
DriverDerivsToLinks	Y	d2l’[robot](vdriver) -> R^numLinks()
LinksToDrivers	Y	l2d[robot](q) -> R^numDrivers()
LinkDerivsToDrivers	Y	l2d’[robot](vdriver) -> R^numLinks()
ConfigInterpolate	N	qinterp[robot](a,b,u) -> R^n

Note that each call to WorldX or LocalX will recompute the forward kinematics of the robot, which can be rather expensive if many of these will be called.

Also, see the KinematicsBuilder class which creates the computational graph for a robot’s forward kinematics, which allows you to reuse subexpressions for multiple forward kinematics calls.

Classes:

ConfigInterpolate(robot)	Autodiff wrapper of the RobotModel.interpolate function
DriverDerivsToLinks(robot)	Autodiff function to convert driver velocities to link velocities.
DriversToLinks(robot)	Autodiff function to convert driver values to link values.
KinematicsBuilder(robot[, configuration, …])	A class that computes the entire computation graph of forward kinematics and caches it so that multiple queries are auto-diffable and share the same intermediate computations.
LinkDerivsToDrivers(robot)	Autodiff function to convert link velocities to driver velocities.
LinksToDrivers(robot)	Autodiff function to convert link values to driver values.
WorldAngularVelocity(link)	Autodiff wrapper of the link.getAngularVelocity() function, as a function of robotconfiguration q and velocity dq.
WorldDirection(link, localDir)	Autodiff wrapper of the link.getWorldDirection() function as a function of robot configuration q.
WorldOrientation(link)	Autodiff wrapper of the link.getTransform()[0] function as a function of robot configuration q.
WorldPosition(link, localPos)	Autodiff wrapper of the link.getWorldPosition() function as a function of robot configuration q.
WorldTransform(link[, localPos])	Autodiff wrapper of the link.getTransform() function as a function of robot configuration q.
WorldVelocity(link, localPos)	Autodiff wrapper of the link.getPointVelocity() function as a function of robot configuration q and velocity dq.

class klampt.math.autodiff.kinematics_ad.ConfigInterpolate(robot)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff wrapper of the RobotModel.interpolate function

Methods:

eval(a, b, u)	Evaluates the application of the function to the given (instantiated) arguments.
n_args()	Returns the number of arguments.
n_in(arg)	Returns the number of entries in argument #arg.
n_out()	Returns the number of entries in the output of the function.

eval(a, b, u)

Evaluates the application of the function to the given (instantiated) arguments.

Parameters

args (list) – a list of arguments, which are either ndarrays or scalars.

n_args()

Returns the number of arguments.

n_in(arg)

Returns the number of entries in argument #arg. If 1, this can either be a 1-D vector or a scalar. If -1, the function can accept a variable sized argument.

n_out()

Returns the number of entries in the output of the function. If -1, this can output a variable sized argument.

class klampt.math.autodiff.kinematics_ad.DriverDerivsToLinks(robot)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff function to convert driver velocities to link velocities.

Methods:

eval(vdriver)	Evaluates the application of the function to the given (instantiated) arguments.
jvp(arg, dvdriver, vdriver)	Performs a Jacobian-vector product for argument #arg.
n_args()	Returns the number of arguments.
n_in(arg)	Returns the number of entries in argument #arg.
n_out()	Returns the number of entries in the output of the function.

eval(vdriver)

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, dvdriver, vdriver)

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

n_args()

Returns the number of arguments.

n_in(arg)

Returns the number of entries in argument #arg. If 1, this can either be a 1-D vector or a scalar. If -1, the function can accept a variable sized argument.

n_out()

Returns the number of entries in the output of the function. If -1, this can output a variable sized argument.

class klampt.math.autodiff.kinematics_ad.DriversToLinks(robot)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff function to convert driver values to link values.

Methods:

eval(qdriver)	Evaluates the application of the function to the given (instantiated) arguments.
jvp(arg, dqdriver, qdriver)	Performs a Jacobian-vector product for argument #arg.
n_args()	Returns the number of arguments.
n_in(arg)	Returns the number of entries in argument #arg.
n_out()	Returns the number of entries in the output of the function.

eval(qdriver)

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, dqdriver, qdriver)

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

n_args()

Returns the number of arguments.

n_in(arg)

Returns the number of entries in argument #arg. If 1, this can either be a 1-D vector or a scalar. If -1, the function can accept a variable sized argument.

n_out()

Returns the number of entries in the output of the function. If -1, this can output a variable sized argument.

class klampt.math.autodiff.kinematics_ad.KinematicsBuilder(robot, configuration='fixed', velocity=None, relative_transforms='fixed', axes='fixed')

Bases: object

A class that computes the entire computation graph of forward kinematics and caches it so that multiple queries are auto-diffable and share the same intermediate computations.

Parameters

• robot (RobotModel) – the robot

• configuration (array, AD expression, or list of expressions, optional) – the robot’s configuration, either as a fixed configuration or a variable. By default, this is fixed at the robot’s configuration.

• velocity (array, AD expression, or list of expressions, optional) – if given, the X_velocity methods are available. This gives the robot’s velocity, either as a fixed vector or a variable. By default, no velocity expression tree is created.

• relative_transform (array or list of AD so3 expressions, optional) – if given, the relative transforms of the robot’s links. By default these are taken from the robot model.

• axes (array or list of AD R^3 expressions, optional) – if given, the axes relative transforms of the robot’s links. By default these are taken from the robot model.

Example:

kb = KinematicsBuilder(robot,'q','dq')
print(kb.world_position(robot.numLinks()-1))
print(kb.world_velocity(robot.numLinks()-1))

Methods:

inv_orientation(link)
local_direction(link, localDir)
local_position(link, localPos)
world_angular_velocity(link)	Returns an autodiff expression for the world angular velocity of the given link.
world_direction(link, localDir)	Returns an autodiff expression for the world direction of the direction localDir on the given link.
world_orientation(link)	Returns an autodiff expression for the orientation of the given link.
world_position(link[, localPos])	Returns an autodiff expression for the world position of the point localPos on the given link.
world_transform(link)	Returns an autodiff expression for the transform of the given link.
world_velocity(link[, localPos])	Returns an autodiff expression for the world velocity of the point localPos on the given link.

inv_orientation(link)

local_direction(link, localDir)

local_position(link, localPos)

world_angular_velocity(link)

Returns an autodiff expression for the world angular velocity of the given link.

Expression evaluates to a 9-D so3_ad array.

world_direction(link, localDir)

Returns an autodiff expression for the world direction of the direction localDir on the given link.

world_orientation(link)

Returns an autodiff expression for the orientation of the given link.

Expression evaluates to a 9-D so3_ad array.

world_position(link, localPos=None)

Returns an autodiff expression for the world position of the point localPos on the given link. If localPos isn’t given, the link origin’s position is returned.

world_transform(link)

Returns an autodiff expression for the transform of the given link.

Expression evaluates to a 12-D se3_ad array.

world_velocity(link, localPos=None)

Returns an autodiff expression for the world velocity of the point localPos on the given link. If localPos isn’t given, the link origin’s velocity is returned.

class klampt.math.autodiff.kinematics_ad.LinkDerivsToDrivers(robot)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff function to convert link velocities to driver velocities.

Methods:

eval(v)	Evaluates the application of the function to the given (instantiated) arguments.
jvp(arg, dv, v)	Performs a Jacobian-vector product for argument #arg.
n_args()	Returns the number of arguments.
n_in(arg)	Returns the number of entries in argument #arg.
n_out()	Returns the number of entries in the output of the function.

eval(v)

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, dv, v)

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

n_args()

Returns the number of arguments.

n_in(arg)

Returns the number of entries in argument #arg. If 1, this can either be a 1-D vector or a scalar. If -1, the function can accept a variable sized argument.

n_out()

Returns the number of entries in the output of the function. If -1, this can output a variable sized argument.

class klampt.math.autodiff.kinematics_ad.LinksToDrivers(robot)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff function to convert link values to driver values.

Methods:

eval(q)	Evaluates the application of the function to the given (instantiated) arguments.
jvp(arg, dq, q)	Performs a Jacobian-vector product for argument #arg.
n_args()	Returns the number of arguments.
n_in(arg)	Returns the number of entries in argument #arg.
n_out()	Returns the number of entries in the output of the function.

eval(q)

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, dq, q)

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

n_args()

Returns the number of arguments.

n_in(arg)

Returns the number of entries in argument #arg. If 1, this can either be a 1-D vector or a scalar. If -1, the function can accept a variable sized argument.

n_out()

Returns the number of entries in the output of the function. If -1, this can output a variable sized argument.

class klampt.math.autodiff.kinematics_ad.WorldAngularVelocity(link)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff wrapper of the link.getAngularVelocity() function, as a function of robotconfiguration q and velocity dq.

Methods:

derivative(arg, q, dq)	Returns the Jacobian of the function w.r.t.
eval(q, dq)	Evaluates the application of the function to the given (instantiated) arguments.
n_args()	Returns the number of arguments.
n_in(arg)	Returns the number of entries in argument #arg.
n_out()	Returns the number of entries in the output of the function.

derivative(arg, q, dq)

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(q, dq)

Evaluates the application of the function to the given (instantiated) arguments.

Parameters

args (list) – a list of arguments, which are either ndarrays or scalars.

n_args()

Returns the number of arguments.

n_in(arg)

Returns the number of entries in argument #arg. If 1, this can either be a 1-D vector or a scalar. If -1, the function can accept a variable sized argument.

n_out()

Returns the number of entries in the output of the function. If -1, this can output a variable sized argument.

class klampt.math.autodiff.kinematics_ad.WorldDirection(link, localDir)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff wrapper of the link.getWorldDirection() function as a function of robot configuration q.

Methods:

derivative(arg, q)	Returns the Jacobian of the function w.r.t.
eval(q)	Evaluates the application of the function to the given (instantiated) arguments.
gen_derivative(arg, q)	Generalized derivative that allows higher-order derivatives to be taken.
n_args()	Returns the number of arguments.
n_in(arg)	Returns the number of entries in argument #arg.
n_out()	Returns the number of entries in the output of the function.

derivative(arg, q)

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(q)

Evaluates the application of the function to the given (instantiated) arguments.

Parameters

args (list) – a list of arguments, which are either ndarrays or scalars.

gen_derivative(arg, q)

Generalized derivative that allows higher-order derivatives to be taken.

Parameters

• arg (list) – Indicates the order of derivatives to be taken. For example, to take the 2nd derivative w.r.t. x0,x1, arg = [0,1] should be specified.

• args (list of ndarrays) – arguments for the function.

Returns

A tensor of shape (n_out,n_in(arg[0]),..., n_in(arg[-1]).

If the generalized derivative is not implemented, raise a NotImplementedError.

If the generalized derivative is zero, can just return 0 (the integer) regardless of the size of the result.

Return type

ndarray

n_args()

Returns the number of arguments.

n_in(arg)

Returns the number of entries in argument #arg. If 1, this can either be a 1-D vector or a scalar. If -1, the function can accept a variable sized argument.

n_out()

Returns the number of entries in the output of the function. If -1, this can output a variable sized argument.

class klampt.math.autodiff.kinematics_ad.WorldOrientation(link)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff wrapper of the link.getTransform()[0] function as a function of robot configuration q.

Methods:

derivative(arg, q)	Returns the Jacobian of the function w.r.t.
eval(q)	Evaluates the application of the function to the given (instantiated) arguments.
gen_derivative(arg, q)	Generalized derivative that allows higher-order derivatives to be taken.
n_args()	Returns the number of arguments.
n_in(arg)	Returns the number of entries in argument #arg.
n_out()	Returns the number of entries in the output of the function.

derivative(arg, q)

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(q)

Evaluates the application of the function to the given (instantiated) arguments.

Parameters

args (list) – a list of arguments, which are either ndarrays or scalars.

gen_derivative(arg, q)

Generalized derivative that allows higher-order derivatives to be taken.

Parameters

• arg (list) – Indicates the order of derivatives to be taken. For example, to take the 2nd derivative w.r.t. x0,x1, arg = [0,1] should be specified.

• args (list of ndarrays) – arguments for the function.

Returns

A tensor of shape (n_out,n_in(arg[0]),..., n_in(arg[-1]).

If the generalized derivative is not implemented, raise a NotImplementedError.

If the generalized derivative is zero, can just return 0 (the integer) regardless of the size of the result.

Return type

ndarray

n_args()

Returns the number of arguments.

n_in(arg)

Returns the number of entries in argument #arg. If 1, this can either be a 1-D vector or a scalar. If -1, the function can accept a variable sized argument.

n_out()

Returns the number of entries in the output of the function. If -1, this can output a variable sized argument.

class klampt.math.autodiff.kinematics_ad.WorldPosition(link, localPos)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff wrapper of the link.getWorldPosition() function as a function of robot configuration q.

Methods:

derivative(arg, q)	Returns the Jacobian of the function w.r.t.
eval(q)	Evaluates the application of the function to the given (instantiated) arguments.
gen_derivative(arg, q)	Generalized derivative that allows higher-order derivatives to be taken.
n_args()	Returns the number of arguments.
n_in(arg)	Returns the number of entries in argument #arg.
n_out()	Returns the number of entries in the output of the function.

derivative(arg, q)

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(q)

Evaluates the application of the function to the given (instantiated) arguments.

Parameters

args (list) – a list of arguments, which are either ndarrays or scalars.

gen_derivative(arg, q)

Generalized derivative that allows higher-order derivatives to be taken.

Parameters

• arg (list) – Indicates the order of derivatives to be taken. For example, to take the 2nd derivative w.r.t. x0,x1, arg = [0,1] should be specified.

• args (list of ndarrays) – arguments for the function.

Returns

A tensor of shape (n_out,n_in(arg[0]),..., n_in(arg[-1]).

If the generalized derivative is not implemented, raise a NotImplementedError.

If the generalized derivative is zero, can just return 0 (the integer) regardless of the size of the result.

Return type

ndarray

n_args()

Returns the number of arguments.

n_in(arg)

Returns the number of entries in argument #arg. If 1, this can either be a 1-D vector or a scalar. If -1, the function can accept a variable sized argument.

n_out()

Returns the number of entries in the output of the function. If -1, this can output a variable sized argument.

class klampt.math.autodiff.kinematics_ad.WorldTransform(link, localPos=None)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff wrapper of the link.getTransform() function as a function of robot configuration q.

Methods:

derivative(arg, q)	Returns the Jacobian of the function w.r.t.
eval(q)	Evaluates the application of the function to the given (instantiated) arguments.
gen_derivative(arg, q)	Generalized derivative that allows higher-order derivatives to be taken.
n_args()	Returns the number of arguments.
n_in(arg)	Returns the number of entries in argument #arg.
n_out()	Returns the number of entries in the output of the function.

derivative(arg, q)

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(q)

Evaluates the application of the function to the given (instantiated) arguments.

Parameters

args (list) – a list of arguments, which are either ndarrays or scalars.

gen_derivative(arg, q)

Generalized derivative that allows higher-order derivatives to be taken.

Parameters

• arg (list) – Indicates the order of derivatives to be taken. For example, to take the 2nd derivative w.r.t. x0,x1, arg = [0,1] should be specified.

• args (list of ndarrays) – arguments for the function.

Returns

A tensor of shape (n_out,n_in(arg[0]),..., n_in(arg[-1]).

If the generalized derivative is not implemented, raise a NotImplementedError.

If the generalized derivative is zero, can just return 0 (the integer) regardless of the size of the result.

Return type

ndarray

n_args()

Returns the number of arguments.

n_in(arg)

Returns the number of entries in argument #arg. If 1, this can either be a 1-D vector or a scalar. If -1, the function can accept a variable sized argument.

n_out()

Returns the number of entries in the output of the function. If -1, this can output a variable sized argument.

class klampt.math.autodiff.kinematics_ad.WorldVelocity(link, localPos)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff wrapper of the link.getPointVelocity() function as a function of robot configuration q and velocity dq.

Methods:

derivative(arg, q, dq)	Returns the Jacobian of the function w.r.t.
eval(q, dq)	Evaluates the application of the function to the given (instantiated) arguments.
n_args()	Returns the number of arguments.
n_in(arg)	Returns the number of entries in argument #arg.
n_out()	Returns the number of entries in the output of the function.

derivative(arg, q, dq)

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(q, dq)

Evaluates the application of the function to the given (instantiated) arguments.

Parameters

args (list) – a list of arguments, which are either ndarrays or scalars.

n_args()

Returns the number of arguments.

n_in(arg)

Returns the number of entries in argument #arg. If 1, this can either be a 1-D vector or a scalar. If -1, the function can accept a variable sized argument.

n_out()

Returns the number of entries in the output of the function. If -1, this can output a variable sized argument.