Python Script to Transform Point from Rigid Body Frame to Ground Frame

[Maximillian Diaz]

Hi Everyone,

I am trying to write a python script in 4.4 that takes a point in a rigid body's reference frame and gives me that point in the ground reference frame throughout a motion to eventually apply a point force. I've done this with Point Kinematics under the Analyze Tool, but OpenSim seems to think it needs 30 minutes to a couple hours initializing the Analyze Tool before it runs the tool which then only takes a few minutes. So...the plan is to bypass the analyze tool and write a script that just pulls and applies the necessary transformation matrices. Unfortunately, I've hit a roadblock with my code and was hoping for some input.

Right now, I think the issue is with how I am trying to pull the transformation matrix of my rigid body with respect to ground. I figured using the getTransformInGround or findTransformBetween would save hard coding all the various transforms between rigid bodies. So far my results just look like noise compared to using the Point Kinematics results as truth (figures below). At first I thought I was using the wrong matrix multiplier function, could still be, but I tried all combinations of them (commented below). Then tried converting the opensim transform variable into a numpy arrays but got consistent results between the numpy array and opensim scripts. I did confirm that the joint coordinates of each model state matches the motion file.

Any thoughts greatly appreciated

Code: Select all

import opensim as osim
import numpy as np
import pandas as pd

# Load and Initialize the model
model = osim.Model('MyModel.osim')
state = model.initSystem()

#Load Motion
q = osim.TimeSeriesTable('MyMotion.mot')
time = q.getIndependentColumn()

# Define the point in the frame of the rigid body
local_point = osim.Vec3(0.00456791, -0.0194644, -0.0088202)  # Point
rigid_body_name = 'distal_thumb'   # Rigid body name

# Variable to store transformed point values
transformed_points=[]

for i,ii in enumerate(time):
    value=osim.RowVector(q.getRowAtIndex(i))
    for j, coordinate in enumerate(model.updCoordinateSet()):
        coordinate.setValue(state, value[j], False)
        
    model.realizePosition(state)
        
    # Get the rigid body
    rigid_body = model.getBodySet().get(rigid_body_name)
    
    # Get the transformation from the local frame to ground frame
    transform=rigid_body.getTransformInGround(state)
    #transform = rigid_body.findTransformBetween(state,model.getGround())
    
	# Apply transformation matrix to point
    #point_in_ground=transform.xformFrameVecToBase(local_point)
    #point_in_ground=transform.xformBaseVecToFrame(local_point)
    point_in_ground=transform.shiftBaseStationToFrame(local_point)
    #point_in_ground=transform.shiftFrameStationToBase(local_point)
    

    # Store the transformed point
    transformed_points.append([time[i],point_in_ground.get(0), point_in_ground.get(1), point_in_ground.get(2)])

#Convert Transformed points into dataframe for saving    
transformed_points=pd.DataFrame(transformed_points, columns=['time','state_0','state_1','state_2'])

State_1.png (56.91 KiB) Viewed 512 times

State_2.png (54.09 KiB) Viewed 512 times

[Nicos Haralabidis]

Hello Maximillian,

Have you tried:

Code: Select all

findStationLocationInGround

https://simtk.org/api_docs/opensim/api_ ... c727289274

You need to first get the frame object for which your point is locally expressed in. That should do the trick.

Best wishes,

Nicos

[Maximillian Diaz]

Hi Nicos,

I got a chance to try your recommendation today...and I think my issue may be with either pulling the correct frame for the rigid body or something with how the frame is being adjusted when I realize the position. I get the same result (figure below) whether I apply findStationLocationInGround to the rigid bodies physical frame or applying it to the base frame pulled from the rigid body. I got curious thinking it may be with how the frame is moving so I tried gerPositionInGround to find the position of the rigid bodies frame in ground...and it is identical to my results from my tries using findStationLocationInGround (same figure below).

Figure 2024-09-30 132225.png (55.98 KiB) Viewed 348 times

Code: Select all

import opensim as osim
import opensim as osim
import numpy as np
import pandas as pd

# Load and Initialize the model
model = osim.Model('MyModel.osim')
state = model.initSystem()

#Load Motion
q = osim.TimeSeriesTable('MyMotion.mot')
time = q.getIndependentColumn()

# Define the point in the frame of the rigid body
local_point = osim.Vec3(0.00456791, -0.0194644, -0.0088202)  # Point
rigid_body_name = 'distal_thumb'   # Rigid body name

# Variable to store transformed point values
transformed_points=[]


for i,ii in enumerate(time):
    value=osim.RowVector(q.getRowAtIndex(i))
    for j, coordinate in enumerate(model.updCoordinateSet()):
        coordinate.setValue(state, value[j], False)
        
    model.realizePosition(state)
        
    # Get the rigid body and frame
    rigid_body = model.getBodySet().get(rigid_body_name)
    body_frame = rigid_body.findBaseFrame()
       
    point_in_ground=body_frame.findStationLocationInGround(state,local_point) #pulling the base frame of the rigid body
    #point_in_ground=rigid_body.findStationLocationInGround(state,local_point) #Using the rigid body physical frame
    #point_in_ground=rigid_body.getPositionInGround(state) #Test to see if position of frame is moving
    
# Store the transformed point
    transformed_points.append([time[i],point_in_ground.get(0), point_in_ground.get(1), point_in_ground.get(2)])

#Convert Transformed points into dataframe for saving    
transformed_points=pd.DataFrame(transformed_points, columns=['time','state_0','state_1','state_2'])

Thanks for your help.

Cheers,
-Max

[Nicos Haralabidis]

Hello Max!

Are the angular coordinates in 'MyMotion.mot' in radians? They are expected to be in radians. Get and print the values of the states after you have set them for a given time point to ensure you have set them accordingly.

Thanks,

Nicos

[Maximillian Diaz]

Hi Nico,

I had been using the direct output from my IK simulations which where in degrees. Changing them to radians did the trick. Thanks for all the help.

-Max