I have a model with a multibar linkage and when I calculate the jacobian for one of the feet the matrix I get back seems to be incorrect. Namely the sign of one of the actuated joints is opposite of what it should be. In reality the linkages are what causes positive motor movement to equal positive Z movement (for example) but the jacobian returned indicates the opposite relationship. In the model the linkages are held together by equality constraints. Are these constraints ignored when the jacobian is calculated? If so is there any other way besides finite differences to calculate the jacobian? Thanks!
The constraints are indeed ignored. The Jacobian is computed as if the model is a kinematic tree. The constraint solver adds the necessary forces to keep the constraints satisfied (softly) but this is not reflected in the Jacobian computation. Apart from finite differences, you could get the unconstrained Jacobian and also get the Jacobian of the equality constraint from mjData.efc_J. Now you can do some linear algebra to obtain the constrained Jacobian analytically.
For what I was doing before I was able to use ObjectVel function instead... but now I am back to needing the jacobian. Preferably analytically since I will need to calculate J_dot as well. So I can get the jacobian of the constraints from efc_J and the unconstrained jacobian from mj_jac... but it isn't clear to me how to combine these to get the constrained jacobian. Is it simple enough to write out the equation here? If not is there some resource that you have found useful in the past?
For the unconstrained system, the velocity mapping is: x = J v But now you want to impose an equality constraint on x: A x = b You can do this by minimizing norm(x - J v) subject to A x = b If you use the squared distance as the norm, this becomes an equality-constrained convex quadratic program. It has an analytical solution which you can find in various tutorials online (or even better, derive it yourself using Lagrange multipliers).
Actually the problem you need to solve is somewhat different. The unconstrained mapping is x = J v. The linear equality constraint however is on v and not x: A v = 0. This assumes that the constraints are never violated, so there is no velocity in the constraint space. But what if you start with a v that violates the constraints? First map that v into a z which satisfies the constraints, by minimizing norm(z - v) s.t. A z = 0, and then map z to x. The resulting constrained Jacobian mapping is: x = J * (I - A' * inv(A * A') * A) * v