time derivative of the ball constraint

[Kaiwen Yang]

Hi,

I am working on an OpenSim model that involves a point constraint (which is implemented as a ball constraint in Simbody). I have a question regards to the method "calcPositionDotErrorsVirtual".

Why the derivative of the constraint error is calculated as :

Code: Select all

Vec3::updAs(&pverr[0]) = v_AS - v_AC;

instead of

Code: Select all

Vec3::updAs(&pverr[0]) = v_AS - v_AP;

The description of the ball constraint is as follow:

// We have a ball joint between base body B and follower body F, located at a
// point P fixed to B and point S fixed on F. All forces will be applied at
// point S and the coincident material point C on B which is instantaneously at
// the same spatial location as S. We will work in the A frame.

I think P and C are both on body B but they are not exactly the same point when constraint is not satisfied, or am I missing something here? Shouldn't the point P on B and point S on F glued together (having zero relative velocity and relative acceleration?)

Given a set of Qs that does not satisfy ball constraint, descrepancy is found between the jacobian I calculated using finte differencing (by repeatedly calling cosntraint::getPositionErrorsAsVector) and Simbody computed constraint jacobian matrix (using method constraint::calcPositionConstraintMatrixPNInv). However, the difference of the two goes to zero when model is at a state when constraint is satisfied.


Kaiwen Yang

[Michael Sherman]

Thanks for this great question, Kaiwen.

Here is our thinking behind this constraint:

Simbody constraints always model some physically-possible system, always assuming that the position and velocity constraints can't be exactly satisfied (they never are in practice). It is easiest to think of this by imagining the system as very far from satisfied, that is with a large perr meaning that points P and S are far apart. We know from Newton's 3rd law that forces between two bodies at a point must be equal and opposite at that point. If the velocity constraint were written as you suggested the two forces would be applied at different points. Instead we must find the point in space at which the two bodies are touching and generate forces there. There is some choice to be made there -- perhaps the optimal one would be (P+S)/2, halfway between the two points that are supposed to be coincident. However, that's a difficult choice to implement and probably not better in practice than choosing one of P and S and applying both forces there. In any case, to satisfy Newton's 3rd, there can only be one point in space where the bodies touch. (And if you don't satisfy Newton's laws, the system is non-physical so won't necessarily conserve energy.) Forces are generated from the acceleration-level constraints.

I think it would be possible to write a constraint using forces at P and S, but then you would need also to include a moment to account for the separation and then it is not clear what the physical mechanism would be that enforces the constraint.

With that as preamble, here is the documentation from the implementation (from ConstraintImpl.h:1870):

Code: Select all

// We have a ball joint between base body B and follower body F, located at a 
// point P fixed to B and point S fixed on F. All forces will be applied at 
// point S and the coincident material point C on B which is instantaneously at
// the same spatial location as S. We will work in the A frame.
//
//  First, find the material point C of B that is coincident
//  in space with point S of F: p_BC = p_AS-p_AB. This vector
//  is *constant* in the B frame because it is a material point,
//  despite the fact that its definition involves a point which
//  moves with respect to B. The velocity constraint is then
//  very simple: the spatial velocity of point C of B should be
//  the same as the spatial velocity of point S of F:
//      verr = v_AS - v_AC = v_AS - (v_AB + w_AB X p_BC) = 0
//  Integrating to get perr, we get
//      perr = p_AS - p_AC + constant = 0
//  But p_AC=p_AS by construction, so perr=constant=0.
//  The constant is defined by the fact that we want material point
//  C of B to be in the same spatial location as point P of B,
//  so constant=p_BC-p_BP=0. Writing in the A frame we have:
//      perr = p_AS-(p_AB+R_AB*p_BP) = 0 (a constant)
//      verr = v_AS - (v_AB + w_AB X R_AB*p_BC)
//      aerr = a_AS - (a_AB + b_AB X R_AB*p_BC + w_AB X (w_AB X R_AB*p_BC))
//  apply +lambda to S of F, -lambda to C of B.

Happy to discuss further.

Regards,
Sherm

[Kaiwen Yang]

Hi Sherm,

Thanks for your detailed answer. This makes sense to me.

I am assuming the same logic can be applied to other types of constraints, for example, point on a surface constraint. Then why are we using velocity errors to calculate constraint matrix P? Isn't this only true when the constraint is satisfied?

Code: Select all

Matrix Constraint::calcPositionConstraintMatrixP(const State& s) const {
    int mp,mv,ma;
    getNumConstraintEquationsInUse(s, mp, mv, ma);

    const SimbodyMatterSubsystem& matter = getMatterSubsystem();
    const System&                 system = matter.getSystem();

    const int nu = matter.getNU(s);

    Matrix P(mp, nu);
    if (mp && nu) {
        Vector  pverr0(mp), pverr(mp); // we're interested in the first mp of these
        State   tmp = s;      // don't change s

        matter.updU(tmp) = 0;   // first calculate the bias term -c(t,q)
        system.realize(tmp, Stage::Velocity);
        pverr0 = getVelocityErrorsAsVector(tmp)(0,mp);

        // Now calculate sensitivity of d(perr)/dt=Pu-c(t,q) to each u in turn.
        for (int j=0; j<nu; ++j) {
            matter.updU(tmp)[j] = 1;
            system.realize(tmp, Stage::Velocity);
            pverr = getVelocityErrorsAsVector(tmp)(0,mp);
            matter.updU(tmp)[j] = 0;
            P(j) = pverr - pverr0;
        }
    }
    return P;
}

[Michael Sherman]

Another good question, thanks Kaiwen. I had to think about this one for a while since it has been a long time!

This comes from Simbody's definition of the position constraint matrix P, based on the intended use in computation. You can see the definition in Doxygen here (scroll down to "Theory discussion"). This matrix is the Jacobian of the linear acceleration constraint that comes from differentiating the velocity constraint. (For a holonomic constraint it's cheaper to calculate P from the velocity constraint than the acceleration constraint which would produce the same result.)

When you calculate a Jacobian by perturbing the position error, you are picking up a term due to the motion of point C on body B (because point S moved). That term is not included in the constraint enforced by Simbody as there is no physical force associated with the choice of C. That's why the hand-wavy derivation kept saying "constant" for terms involving C. At any instant we choose a material point of B at which to write the velocity and then acceleration constraint that ultimately produces forces. For the purpose of the constraint, C is just a constant so we don't differentiate its position to get the V and A constraints.

There is very likely also a use for J=partial(perr)/partial(q) (where C is allowed to move) but that's not how we define P.

I'm interested in your thoughts on this because constraint literature almost always presents derivations assuming the constraints are perfectly satisfied (based presumably on the idea that they are going to be almost satisfied so the errors are negligible). I never found that satisfactory as a way to think about computational methods where constraint violations can be quite substantial, especially when running fast at loose accuracies.

Sherm

[Kaiwen Yang]

Hi Sherm,

Thanks for your explanation. I am not an expert in dynamics but from my application, I need to solve a feasible set of q, u, and udot from a potentially infeasible set of initial guess using gradient based optimizer. So I need to supply both J (partial(perr)/partial(q)) and P to the solver. I was expecting these two matrices are eactly the same .However, from our conversation, now I can live with the fact that they are not when qerr is not zero. I think I can get J using other means such as station jacobian. Its all good.

My problem with simbody's effort maintaining the system "physically-possible" is that it is not generalized enough. What if they constraint is abstract rather than a simple ball constraint? Let's say the state of the system is moving on some smooth constraint manifold in state space. At every instance, the matrix P sort of represent the tangent plane (by identifying a set of basis of the plane) on the surface. Now the question is, how to define the tangent plane when the system is off the smooth surface? I guess there is no perfect answer...

Kaiwen

[Michael Sherman]

Agreed, there is no one-size-fits-all perfect solution. In Simbody I biased my choices to modeling some physical system at all times. That allows validation based on physical laws like f=ma, conservation of energy, etc.

A comment on finding feasible solutions: Simbody has an Assembler study for finding a feasible configuration. That uses P as the Jacobian (actually P N^-1) and works quite well in practice, even starting from a very bad q. The full partial(perr)/partial(q) approaches PN^-1 as perr->0 so the direction is not far off for solving the nonlinear problem.

Sherm