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().
GravityVector(robot)	Autodiff wrapper of RobotModel.getGravityForces().
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.
CoriolisVector(robot)	Autodiff wrapper of RobotModel.getCorolisForces().
ForwardDynamics(robot)	Autodiff function to compute the forward dynamics ddq = f(q,dq,t).
InverseDynamics(robot)	Autodiff function to compute the inverse dynamics t = f(q,dq,ddq).
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.
FullDynamics(robot, g[, links, localpts])	Autodiff function that computes the standard forward dynamics equation:

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

Bases: ADFunctionInterface

Autodiff wrapper of RobotModel.getCom().

Methods:

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

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.

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

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

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

Bases: ADFunctionInterface

Autodiff wrapper of RobotModel.getGravityForces().

Methods:

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

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.

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

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

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

Bases: ADFunctionInterface

Autodiff wrapper of RobotModel.getKineticEnergy().

Methods:

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

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.

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.

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

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

Bases: ADFunctionInterface

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

Methods:

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

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.

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.

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

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

Bases: ADFunctionInterface

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

Methods:

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.

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.

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.

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

Bases: ADFunctionInterface

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

Methods:

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

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.

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.

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

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

Bases: ADFunctionInterface

Autodiff wrapper of RobotModel.getCorolisForces().

Methods:

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.

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.

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.

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

Bases: 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:

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.

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.

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.

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

Bases: 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:

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.

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.

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.

class klampt.math.autodiff.dynamics_ad.PointForceTorques(link, localpt)

Bases: 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:

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

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

class klampt.math.autodiff.dynamics_ad.PointWrenchTorques(link, localpt)

Bases: 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:

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

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

class klampt.math.autodiff.dynamics_ad.FullDynamics(robot, g, links=None, localpts=None)

Bases: 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:

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.

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.

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.