Extract individual contact forces and positions from Moco solution

[Delyle Polet]

I have a rimless wheel model with many contact elements. I want to extract the contact forces from these individual elements and visualize them in the GUI.

Is there a function in Moco that can do this?

I've tried using the opensimMoco.createExternalLoadsTableForGait, but this creates a single vector for left and right contact elements. It seems that the individual contact forces are embedded somewhere in the moco solution, as study.visualize(solution) displays the forces individually and in the correct place.

Is there a workaround with other tools? It seems it should be possible to use the Analyze tool with a ForceReporter, but I haven't been able to get it to work.

Thanks for your help!

[Nicholas Bianco]

Hi Delyle,

Unfortunately, some of the force components do not work directly with the reporting utilities in Moco (something I'd like to fix soon). Probably the best way to do what you're looking for is to copy what createExternalLoadsTableForGait is doing under the hood.

In short,

• Convert your MocoTrajectory to a StatesTrajectory using exportToStatesTrajectory().

• Realize your model to the Velocity stage using each State in the StatesTrajectory.

• Get the force components using getComponent<Force>(). (You need to use getComponent(), then safeDownCast in Matlab).

• Compute forces using getRecordValues().

-Nick

[Delyle Polet]

Thank you Nick! I'm able to get forces that way. I'll just need to work through getting the position of the contact elements now to work out GRF position

Delyle

[Nicholas Bianco]

Oh, I see. I actually needed to do the same for a project of my own recently. Below are two Python functions I wrote to compute individual contact sphere forces, contact sphere force locations, centers of pressure, and the typical OpenSim external loads table. You'll need to make some changes if you're using Matlab, but these should include everything you need.

At some point I'll turn this into a Moco utility.

Code: Select all

    def create_contact_sphere_force_table(self, model, solution):
        model.initSystem()
        externalForcesTable = osim.TimeSeriesTableVec3()
        copTable = osim.TimeSeriesTableVec3()
        statesTrajectory = solution.exportToStatesTrajectory(model);
        numStates = statesTrajectory.getSize() 

        forceNames = ['forceset/contactHeel',
                      'forceset/contactLateralRearfoot',
                      'forceset/contactLateralMidfoot',
                      'forceset/contactLateralToe',
                      'forceset/contactMedialToe',
                      'forceset/contactMedialMidfoot'
                      ]

        forceLabels = ['heel',
                       'lat_rear',
                       'lat_mid', 
                       'lat_toe', 
                       'med_toe', 
                       'med_mid',
                       ]

        sphereNames = ['contactgeometryset/heel',
                       'contactgeometryset/lateralRearfoot',
                       'contactgeometryset/lateralMidfoot',
                       'contactgeometryset/lateralToe',
                       'contactgeometryset/medialToe',
                       'contactgeometryset/medialMidfoot',
                       ]

        for istate in range(numStates):

            state = statesTrajectory.get(istate)
            model.realizeVelocity(state)

            row = osim.RowVectorVec3(3*2*len(forceNames))
            copRow = osim.RowVectorVec3(2)
            labels = osim.StdVectorString()
            for iside, side in enumerate(['_r', '_l']):
                
                cop = osim.Vec3(0)
                copTorqueSum_x = 0 
                copForceSum = 0
                copTorqueSum_z = 0

                offset = 3 * iside * len(sphereNames)
                zipped = zip(forceNames, forceLabels, sphereNames)
                for i, (forceName, forceLabel, sphereName) in enumerate(zipped):
                    force = osim.Vec3(0)
                    torque = osim.Vec3(0)

                    forceObj = osim.Force.safeDownCast(
                        model.getComponent(f'{forceName}{side}'))
                    forceValues = forceObj.getRecordValues(state)

                    force[0] = forceValues.get(0)
                    force[1] = forceValues.get(1)
                    force[2] = forceValues.get(2)
                    torque[0] = forceValues.get(3)
                    torque[1] = forceValues.get(4)
                    torque[2] = forceValues.get(5)

                    sphere = osim.ContactSphere.safeDownCast(
                        model.getComponent(f'{sphereName}{side}'))
                    frame = sphere.getFrame()
                    position = frame.getPositionInGround(state)
                    location = sphere.get_location()
                    locationInGround = frame.expressVectorInGround(state, location)

                    position[0] = position[0] + locationInGround[0]
                    position[1] = position[1] + locationInGround[1]
                    position[2] = position[2] + locationInGround[2]

                    row[3*i + offset] = force
                    row[3*i + 1 + offset] = position
                    row[3*i + 2 + offset] = torque

                    copForceSum += force[1]
                    copTorqueSum_x += force[1] * position[0]
                    copTorqueSum_z += force[1] * position[2]

                    for suffix in ['_force_v', '_force_p', '_torque_']:
                        labels.append(f'{forceLabel}{side}{suffix}')

                if np.abs(copForceSum) > 1e-3:
                    cop[0] = copTorqueSum_x / copForceSum
                    cop[2] = copTorqueSum_z / copForceSum
                
                copRow[iside] = cop

            externalForcesTable.appendRow(state.getTime(), row)
            copTable.appendRow(state.getTime(), copRow)

        externalForcesTable.setColumnLabels(labels)

        labels = osim.StdVectorString()
        labels.append('ground_force_r_p')
        labels.append('ground_force_l_p')
        copTable.setColumnLabels(labels)

        suffixes = osim.StdVectorString()
        suffixes.append('x')
        suffixes.append('y')
        suffixes.append('z')

        return externalForcesTable.flatten(suffixes), copTable

    
    def create_external_loads_table_for_gait(self, model,
            solution, forcePathsRightFoot, forcePathsLeftFoot,
            copTable):

        model.initSystem()
        externalForcesTable = osim.TimeSeriesTableVec3()
        statesTrajectory = solution.exportToStatesTrajectory(model);
        numStates = statesTrajectory.getSize() 

        for istate in range(numStates):

            state = statesTrajectory.get(istate)
            model.realizeVelocity(state)

            forcesRight = osim.Vec3(0)
            torquesRight = osim.Vec3(0)

            # Loop through all Forces of the right side.
            for smoothForce in forcePathsRightFoot:
                force = osim.Force.safeDownCast(model.getComponent(smoothForce))
                forceValues = force.getRecordValues(state)
                forcesRight[0] = forcesRight[0] + forceValues.get(0)
                forcesRight[1] = forcesRight[1] + forceValues.get(1)
                forcesRight[2] = forcesRight[2] + forceValues.get(2)
                torquesRight[0] = torquesRight[0] + forceValues.get(3)
                torquesRight[1] = torquesRight[1] + forceValues.get(4)
                torquesRight[2] = torquesRight[2] + forceValues.get(5)

            forcesLeft = osim.Vec3(0)
            torquesLeft = osim.Vec3(0)

            # Loop through all Forces of the left side.
            for smoothForce in forcePathsLeftFoot:
                force = osim.Force.safeDownCast(model.getComponent(smoothForce))
                forceValues = force.getRecordValues(state)
                forcesLeft[0] = forcesLeft[0] + forceValues.get(0)
                forcesLeft[1] = forcesLeft[1] + forceValues.get(1)
                forcesLeft[2] = forcesLeft[2] + forceValues.get(2)
                torquesLeft[0] = torquesLeft[0] + forceValues.get(3)
                torquesLeft[1] = torquesLeft[1] + forceValues.get(4)
                torquesLeft[2] = torquesLeft[2] + forceValues.get(5)

            # Get COP values
            copRow = copTable.getRowAtIndex(istate)

            # Append row to table.
            row = osim.RowVectorVec3(6)
            row[0] = forcesRight
            row[1] = copRow[0]
            row[2] = forcesLeft
            row[3] = copRow[1]
            row[4] = torquesRight
            row[5] = torquesLeft
            externalForcesTable.appendRow(state.getTime(), row);
        
        # Create table.
        labels = osim.StdVectorString()
        labels.append('ground_force_r_v')
        labels.append('ground_force_r_p')
        labels.append('ground_force_l_v')
        labels.append('ground_force_l_p')
        labels.append('ground_torque_r_')
        labels.append('ground_torque_l_')

        externalForcesTable.setColumnLabels(labels)

        suffixes = osim.StdVectorString()
        suffixes.append('x')
        suffixes.append('y')
        suffixes.append('z')

        return externalForcesTable.flatten(suffixes)
 

[Nicholas Vandenberg]

Hi,
I was hoping I could jump into this thread with a question about converting these from Python to Matlab. I've seen/used 'create_external_loads_table_for_gait' before but not 'create_contact_sphere_force_table', which seems to be doing exactly what I'm looking for. I'm not particularly savvy with Python though, so I'm having some issues with making a working conversion to Matlab. Mostly indexing errors with the Vec3's ("Java arrays can't be filled with doubles") but in general I can't seem to get anything working. Wondering if there's any tips?

Thanks,
Nick

[Nicholas Bianco]

Hi Nick,

I've recently added this functionality directly to OpenSim/Moco. You should get center of pressure values automatically when you call createExternalLoadsTableForGait. If you download the recent 4.4.1 API release, you can access this feature: https://github.com/opensim-org/opensim- ... /tag/4.4.1.

Best,
Nick

[Nicholas Vandenberg]

Amazing! Thanks Nick!

[Pasha van Bijlert]

Hi Nick,

I'm reviving this thread because I'd like to double check a few things regarding COP. I'm very close to submitting a paper where we've done quadrupedal simulations (using a newly developed model of the horse). I'd like to add a function that precomputes some outputs based on a converged simulation.
I was initially computing COP using a weighted sum of each contact sphere's intersection point with the ground, using vertical force per sphere as the weighting factor. I decided to cross check it with the implementation you've described in this thread, and that I think is listed in the source code here . createExternalLoadsTableForGait only works with two legs, so I've implemented it myself in matlab (extracting HalfSpaceForces and Moments manually, summing them together per foot, and then dividing M_x and M_z by F_y to get COP_x and COP_z, respectively.

I've got a few questions about this, because the COP's we're getting are similar, but not identical (when the foot is on the ground, they're off by a few millimeters).

1) I'm a bit confused by what the ground reaction moments actually represent. I assume they are moments that arise from transforming the ground reaction forces from the contact point to a different reference frame. I initially thought they were transformed to the parent body origin, but I think then the implementation from createExternalLoadsTableForGait wouldn't work without an extra transformation. Are the outputs from force.getRecordValues expressed in the global reference frame?

2) The horizontal force components are assumed to be small / are ignored in this approach, is that correct?

3) Presumably, COP_y = M_y/F_y.

Thanks,
Pasha

[Nicholas Bianco]

Hi Pasha,

Happy to clarify.

1. First note that in order to sum across contact force elements, all COP calculations are based on the half space forces and torques, which all share the same frame (ground) and, for the torques, share the same origin point (the ground origin). If you have contact elements on different bodies (e.g., the calcn and the toes) then summing moments doesn't make sense. If you consider the contact half space as the "force plate", then the ground origin is the "force plate origin". Yes, all forces and torques are expressed in ground.

2. Since the "force plate origin" coincides with the ground origin, the horizontal force components do not appear in the COP calculations. If that were not true, then you would have extra terms including the horizontal and mediolateral forces. See the following reference for more details: http://www.kwon3d.com/theory/grf/cop.html. (Note that this reference uses Z as the vertical axis, not Y).

3. No, we set COP_y to zero. If you don't, you end up with too many unknowns when solving for COP_x and COP_z. Again, I refer you to the link above.

Now that I'm looking at createExternalLoadsTableForGait, I don't think we should be reporting the sphere torques at all, for the reason I mentioned above. We should compute the "vertical moment" and report that instead.

Best,
Nick

[Pasha van Bijlert]

Hi Nick,

Thanks a lot for the clarification!

Since the "force plate origin" coincides with the ground origin, the horizontal force components do not appear in the COP calculations.... No, we set COP_y to zero.

That makes sense, thank you. So you're setting a, b, and c from your link to 0. I think then this also defines all the ground contact deformation to take place in the contact sphere, with an undeformable contact halfspace. If you want to define the sphere and ground to both be deformable, you would then need to add an equation that relates c (from you link) to the contact sphere penetration (this level of detail is definitely unnecessary for my analysis).

We should compute the "vertical moment" and report that instead.

As a check that I'm fully understanding this, essentially you mean Tz = Mz - x * Fy + y * Fx [using Z is up] or Ty = My + x *Fz - z *Fx [using Y is up]. All this assumes we're talking about the halfspaceforces and torques, all expressed in the ground frame with respect to the ground origin.

Thank you, this helped a lot!
Cheers,
Pasha