How to do inverse dynamics?

[ManuelZ]

I would like to prescribe a velocity to an object and measure the force neccesary to achieve this motion.

For example, I would like to make the platform (the empty red rectangle) move vertically at certain speed, and I would like to measure the required force in the actuator (the object composed of gray and orange parts) to achieve this.

To prescribe the velocity, should I use a CoordinateVectorConstraint like in coordinateVectorConstraint.py? I would provide a ground marker and a marker in the platform (maybe located in the center of mass of the platform?).

I tried returning a [0] from UFconstraint (to have no position constraints between the markers) and returning [0, 1] from UFjacobian (to, supposedly, have a 1m/s vertical speed constraint), but I don't think that's how this works, because I couldn't obtain the desired vertical motion at the desired speed in the coordinateVectorConstraint.py file from where this code comes from:

def UFconstraint(mbs, t, itemNumber, q, q_t, velocityLevel):
    return return [np.sqrt(q[0] ** 2 + q[1] ** 2) - L]

def UFjacobian(mbs, t, itemNumber, q, q_t, velocityLevel):
    jac = np.zeros((1, 2))

    f = np.sqrt(q[0] ** 2 + q[1] ** 2)
    jac[0, 0] = q[0] / f
    jac[0, 1] = q[1] / f
    return jac

mbs.AddObject(
    CoordinateVectorConstraint(
        markerNumbers=[mCoordsGround, mCoords0],
        scalingMarker0=np.zeros((1, 2)),  # needed to define number of algebraic equations; rows=nAE, cols=len(q) of mCoordsGround + mCoords0
        constraintUserFunction=UFconstraint,
        jacobianUserFunction=UFjacobian,
        visualization=VCoordinateVectorConstraint(show=False),
    )
)

I understand that what is returned from UFjacobian should be the derivative of the returned value from UFconstraint w.r.t x and y, but in my case I would only return zero from UFconstraint. At this point I don't know how to proceed.

Observations

When reading the docs, I noticed:

1. In p. 688, in the table, in the description of constraintUserFunction it says numpy.zeros((nAE, 1)), but shouldn't it be numpy.zeros((nAE, len(q)))?
2. In p. 689, third table, in the symbol of quadraticTermMarker1, I think $Y_{m0}$ should be $Y_{m1}$ and $q_{m0}$ should be $q_{m1}$
3. In p. 692, in the 'type or size' of the return value of jacobianUserFunction, it says $R^{({n_{q_{m0}} + n_{q_{m1}}}) \times n_{ae}}$, but shouldn't it be $R^{n_{ae} \times ({n_{q_{m0}} + n_{q_{m1}}})}$ ? (i.e. shouldn't $n_{ae}$ be the number of rows, instead of the number of columns?)

[jgerstmayr]

This is a good point. The CoordinateVectorConstraint is intended to be used to add general constraints to the system, such as Euler-Parameter constraints or any other constraints. However, in the present implementation constraints are always related to Lagrange multipliers of the constraint. What you intend to build is a constraint on the motion, related to a Lagrange multiplier (force) of another item in the system.
This is currently not intended to be done, but an interesting option for any inverse dynamics problem. As it adds an algebraic constraint, this has to be treated carefully and in the general case, one would like to prescribe several velocities (e.g. of the end-effector of the robot) using a set of actuator forces. I will put this on my list.
Certainly, you could do everything step-by-step in the time integrator by building some Newton iteration locally in every time step in order to solve for the actuator force for a given velocity. But this would be rather complicated to realize.

[ManuelZ]

Oh I see, ok, thank you.