klampt.math.autodiff.dynamics_ad module

Klampt dynamics AD functions:

Function	Derivative	Notes
CenterOfMass	1	com[robot](q) -> R^3
GravityVector	1	G[robot](q,g) -> R^n
KineticEnergy	1	KE[robot](q,dq) -> R
MassMatrix	1	vec(B)[robot](q) -> R^(n*n)
MassMatrixInv	N	vec(B^-1)[robot](q) -> R^(n*n)
MomentumVector	N	B*v[robot](q,v) -> R^n
CoriolisVector	N	C[robot](q,dq) -> R^n
ForwardDynamics	N	ddq(q,dq,t) -> R^n
InverseDynamics	N	t(q,dq,ddq) -> R^n
PointForceTorques	1	J^T*f[link,localpt](q,f) -> R^n
PointWrenchTorques	1	J^T*w[link,localpt](q,w) -> R^n
FullDynamics	N	ddq[robot,g,links,localpoints](q,dq,t,fs) -> R^n

Classes:

CenterOfMass(robot)	Autodiff wrapper of RobotModel.getCom().
CoriolisVector(robot)	Autodiff wrapper of RobotModel.getCorolisForces().
ForwardDynamics(robot)	Autodiff function to compute the forward dynamics ddq = f(q,dq,t).
FullDynamics(robot, g[, links, localpts])	Autodiff function that computes the standard forward dynamics equation:
GravityVector(robot)	Autodiff wrapper of RobotModel.getGravityForces().
InverseDynamics(robot)	Autodiff function to compute the inverse dynamics t = f(q,dq,ddq).
KineticEnergy(robot)	Autodiff wrapper of RobotModel.getKineticEnergy().
MassMatrix(robot)	Autodiff wrapper of RobotModel.getMassMatrix().
MassMatrixInv(robot)	Autodiff wrapper of RobotModel.getMassMatrixInv().
MomentumVector(robot)	Autodiff function to compute B(q)*v as a function of (q,v) where B(q) is the mass matrix.
PointForceTorques(link, localpt)	Calculates the robot DOF torques as a function of the force f in R^3 on a given point localpt on link link.
PointWrenchTorques(link, localpt)	Calculates the robot DOF torques as a function of the wrench w=(tau,f) in R^6 on a given point localpt on link link.

class klampt.math.autodiff.dynamics_ad.CenterOfMass(robot)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff wrapper of RobotModel.getCom().

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.
jvp(arg, darg, 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.

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.

jvp(arg, darg, 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.dynamics_ad.CoriolisVector(robot)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff wrapper of RobotModel.getCorolisForces().

Methods:

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.

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.dynamics_ad.ForwardDynamics(robot)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff function to compute the forward dynamics ddq = f(q,dq,t).

Note: the torque vector is the full torque vector of length robot.numLinks(). If you want to convert from a driver torque vector, use kinematics_ad.DriverDerivsToLinks.

Methods:

argname(arg)	Returns a descriptive string for argument #arg
eval(q, dq, t)	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.

argname(arg)

Returns a descriptive string for argument #arg

eval(q, dq, t)

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.dynamics_ad.FullDynamics(robot, g, links=None, localpts=None)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff function that computes the standard forward dynamics equation:

\(\ddot{q} = B(q)^{-1} (S au + sum_i J_i(q)^T f_i - C(q,\dot{q}) - G(q))\)

as a function of (q,dq,t,f). Here, B is the mass matrix, t=:math:` au` are the actuated joint torques, S is a selection matrix, the J’s are the Jacobians of the contact points, C is the coriolis vector, and G is the gravity vector corresponding to the external force g.

Note: the torque vector is the reduced torque vector of length robot.numDrivers().

Methods:

argname(i)	Returns a descriptive string for argument #arg
eval(q, dq, t, *f)	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.

argname(i)

Returns a descriptive string for argument #arg

eval(q, dq, t, *f)

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.dynamics_ad.GravityVector(robot)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff wrapper of RobotModel.getGravityForces().

Methods:

derivative(arg, q, g)	Returns the Jacobian of the function w.r.t.
eval(q, g)	Evaluates the application of the function to the given (instantiated) arguments.
jvp(arg, darg, q, g)	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.

derivative(arg, q, g)

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, g)

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

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.dynamics_ad.InverseDynamics(robot)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff function to compute the inverse dynamics t = f(q,dq,ddq).

Note: the torque vector is the full torque vector of length robot.numLinks(). If you want to convert to a driver torque vector, use kinematics_ad.LinkDerivsToDrivers.

Methods:

argname(i)	Returns a descriptive string for argument #arg
eval(q, dq, ddq)	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.

argname(i)

Returns a descriptive string for argument #arg

eval(q, dq, ddq)

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.dynamics_ad.KineticEnergy(robot)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff wrapper of RobotModel.getKineticEnergy().

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.dynamics_ad.MassMatrix(robot)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff wrapper of RobotModel.getMassMatrix(). The result is flattened into a 1D array of length n^2.

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

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.dynamics_ad.MassMatrixInv(robot)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff wrapper of RobotModel.getMassMatrixInv(). The result is flattened into a 1D array of length n^2.

Methods:

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

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.dynamics_ad.MomentumVector(robot)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Autodiff function to compute B(q)*v as a function of (q,v) where B(q) is the mass matrix.

Methods:

derivative(arg, q, v)	Returns the Jacobian of the function w.r.t.
eval(q, v)	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, v)

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, 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.

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.dynamics_ad.PointForceTorques(link, localpt)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Calculates the robot DOF torques as a function of the force f in R^3 on a given point localpt on link link.

Methods:

eval(q, f)	Evaluates the application of the function to the given (instantiated) arguments.
jvp(arg, darg, q, f)	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, f)

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

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.dynamics_ad.PointWrenchTorques(link, localpt)

Bases: klampt.math.autodiff.ad.ADFunctionInterface

Calculates the robot DOF torques as a function of the wrench w=(tau,f) in R^6 on a given point localpt on link link.

Methods:

eval(q, w)	Evaluates the application of the function to the given (instantiated) arguments.
jvp(arg, darg, q, w)	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, w)

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

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.