Problem with the input of coordinates into the IK solver

[Yannick Hemmeter]

Hello,

I am a complete beginner in OpenSim and have a project at university where I have to extract 3D keypoints from a pose estimation model and then use them to determine the inverse kinematic and inverse dynamic in OpenSim. The whole thing is supposed to run in Python.

I have spent the last few days doing a lot of research and found these two programms:

https://github.com/RealTimeBiomechanics ... /README.md
https://github.com/pslade2/RealTimeKin/tree/main

I tried to take the methods from the two programs, but they have a slightly different structure than in my case, they use queues. In my case, however, I would run the calculations sequentially to my Estimetion pose. (I don't know if it would be better to integrate a queue)

Furthermore, I am not sure how to get the coordinates (all values go from 0 to 1) from my Pose Estimation into the IK Solver. I have given a previous code below:

Code: Select all

import NewPoseEstimation as pm
import cv2
import opensim as osim

osimModel = osim.Model("Ergonomicus_DIN33402_Male_P50_OpenSim4.0.osim")
osimModel.setUseVisualizer(True)
state = osimModel.initSystem()
q = state.getQ()
q.setToZero()
state.setQ(q)
modelViz = osimModel.getVisualizer()
viz = modelViz.getSimbodyVisualizer()
viz.report(state)

cap = cv2.VideoCapture(0)
detector = pm.PoseEstimatior()

accuracy = 0.05
constraint_var = 1

def fromArrayStr(array_str):
    try:
        r = [array_str.getitem(i) for i in range(array_str.getSize())]
        return r
    except Exception as err:
        raise err

markerNamesArray = osim.ArrayStr()
osimModel.getMarkerSet().getMarkerNames(markerNamesArray)
markerNames = fromArrayStr(markerNamesArray)
nMarkers = len(markerNames)

coordinateNamesArray = osim.ArrayStr()
osimModel.getCoordinateSet().getNames(coordinateNamesArray)
coordinateNames = fromArrayStr(coordinateNamesArray)
nCoordinates = osimModel.getNumCoordinates()

markerWeights = {marker: 1 for marker in markerNames}

sortedMarkerWeights = [markerWeights[marker] for marker in markerNames]

osimMarkerWeights = osim.SetMarkerWeights()
for marker in markerNames:
    osimMarkerWeights.adoptAndAppend(osim.MarkerWeight(marker, markerWeights[marker]))

markersReference = osim.MarkersReference()
markersReference.setMarkerWeightSet(osimMarkerWeights)

coordinateRefs = osim.CoordinateReference()

#InverseKinematicsSolver (const Model &model, const MarkersReference &markersReference, SimTK::Array_< CoordinateReference > &coordinateReferences, double constraintWeight=SimTK::Infinity)
ikSolver = osim.InverseKinematicsSolver(osimModel, markersReference, coordinateRefs, constraint_var)
ikSolver.setAccuracy(accuracy)
ikSolver.assemble(state)

while cap.isOpened():
    _, img = cap.read()
    try:
        img = detector.findPose(img)
        lmList, complete = detector.getPosition(img)
        if complete:
            #IK
            
        else:
            if previousAngles:
                #Person not in Frame -> Same State as before

        cv2.imshow("Image", img)
    except:
        break
    
    if cv2.waitKey(1) == ord('q'):
        break
    
cap.release()
cv2.destroyAllWindows()

I am of course available for further questions.
Any help would be greatly appreciated.

[Nicholas Bianco]

Hi Yannick,

I tried to take the methods from the two programs, but they have a slightly different structure than in my case, they use queues. In my case, however, I would run the calculations sequentially to my Estimetion pose

I believe these libaries use queues to manager a buffer of markers/keypoints to perform real-time inverse kinematics. If you need to do something similar then you might be able to leverage the queue structure to handle both pose estimation and IK.

As for running the IK solver, what are you stuck on specifically? It seems like you have started to put some of the main pieces in place (e.g., constructing a MarkersReference). For the MarkersReference, you should be able to use the constructor that takes a table of marker data (populated from your real-time keypoint data stream) to run IK:

Code: Select all

MarkersReference(const TimeSeriesTable_<SimTK::Vec3>& markerData,
        const Set<MarkerWeight>& markerWeightSet,
                     Units units = Units(Units::Meters));

Best,
Nick