sensitivity of jacobian transpose force mapping

[Daniel Krueger]

Hi all,
I am working on an inverse kinematics solution that accounts for static equilibrium conditions.
The goal is to find system configurations q that
a) satisfy several kinematic tasks (e.g. reaching for an object)
b) allow the system to be in static equilibrium meaning that in mobility space all forces acting on the system (gravity, actuators, muscles) sum up to zero: sum (tau)=0
Therefore I am looking for an elegant way to form the Jacobian of these equilibrium conditions with respect to the system configuration. Here an example:
To map a force F acting on a rigid body into mobility space (tau) we can use the system jacobian transpose operator: tau=~Jsys*F
What I am heading for is the sensitivity of this mapping with respect to the current configuration q: d (~Jsys*F)/dq !
Is there a way to get this analytically from simbody? My first Idea was to abuse the jacobian "bias" operators of SimbodyMatterSubsystem for this purpose since they compute Jdot*u which is in some way related to dJ/dq....
Best regards,
Daniel

[Michael Sherman]

Hi, Daniel. Calculating da/dq analytically is difficult, but solving a(q)=0 is usually easy to do numerically so you probably don't need an analytic da/dq. You can find static equilibrium using IpOpt, for example, which will compute da/dq numerically when needed. Simbody has a class called LocalEnergyMinimizer that does exactly that, although it formulates the problem as minimizing potential energy rather than finding a=0.

You can form and factor da/dq yourself using Simbody's Differentiator class or with your own perturbation code. The primary difference between an analytic and numerical Jacobian da/dq is the precision with which it can be obtained: 6 decimal places with first order differentiation, 10 with second order, and 14 or so analytically or with automatic differentiation. The numerical methods are usually somewhat faster also.

Even for systems with no constraints a=inv(M)*f is an ugly thing to differentiate. The more technically difficult part is dM/dq but in practice df/dq can be worse since that term involves arbitrary user-written forces.

Regards,
Sherm