How to use TimeSeriesTableQuaternion.appendRow in python?

[Arthur Matta]

Hello,

For reference, I'm using Python 3.8 and OpenSim 4.4.

Running the following code:

Code: Select all

import numpy as np
import opensim as osim

SAMPLE_DATA = np.array(
    [
        [0, 0.9995429673603463, -0.008673246070566128, 0.02895804025163351, -0.0002512127741937707, 0.9988056889348111,
         0.009225910844409206, 0.04797793296728128, 0.0004430222738762587],
        [0.01, 0.9983235155232013, -0.01648178176931649, 0.0554767580235808, -0.0009157195522055893, 0.9953451329047127,
         0.01842024119373089, 0.0945817043153574, 0.001749950304193833],
        [0.02, 0.996553572768251, -0.02364517310391066, 0.0794879005037651, -0.001885753224544679, 0.9898697043858768,
         0.0275592750627042, 0.1392244052239398, 0.003875524103277203],
        [0.03, 0.9944026364977537, -0.03036249384785823, 0.1011532414540192, -0.003088241732745588, 0.9826840433510849,
         0.0366053997686734, 0.1815114641069645, 0.006760476956889426],
        [0.04, 0.9920155995198686, -0.03624118422120523, 0.1207152892736455, -0.004409740475433332, 0.974111394130037,
         0.04551417126443761, 0.22119823325065, 0.01033410116740631],
        [0.05, 0.9894759113314432, -0.04174418491853947, 0.1384223432967164, -0.005839417483162094, 0.9644686836064071,
         0.05440249635598614, 0.2581251100202142, 0.01455865068655974],
        [0.06, 0.9868508854848144, -0.04693285443957362, 0.154494844744801, -0.007347104141869534, 0.9540736552868223,
         0.06326532185989953, 0.2921740174350191, 0.01937273531245459],
        [0.07, 0.9842021159098119, -0.05180189751605759, 0.1690667958671119, -0.00889814517826073, 0.943280037377913,
         0.0721245581574768, 0.3231261113774534, 0.0247049659177489],
        [0.08, 0.9816277378550423, -0.05621921067018604, 0.1820376363912426, -0.01042514087690647, 0.9322749346658372,
         0.08098224817958229, 0.3512470753675765, 0.03050923990061744],
        [0.09, 0.9791306321049124, -0.06029023660482678, 0.1937163440868847, -0.01192772725965339, 0.9212212941768754,
         0.09000505440402377, 0.3766909623592524, 0.03680130722775423],
        [0.10, 0.9767195496681569, -0.06392963022845036, 0.2043356497812238, -0.01337407579504081, 0.9103290086705185,
         0.09894767715988709, 0.399530682092155, 0.04342450028027244]
    ]
)

time = SAMPLE_DATA[:, 0]
thorax_data = SAMPLE_DATA[:, 1:5]
humerus_data = SAMPLE_DATA[:, 5:]

qtable = osim.TimeSeriesTableQuaternion()

for t, thorax, humerus in zip(time, thorax_data, humerus_data):
    qtable.appendRow(t, np.array([thorax, humerus]))

Raises the following error:

Code: Select all

Traceback (most recent call last):
  File "<stdin>", line 2, in <module>
  File "/home/pps4/.pyenv/versions/stv/lib/python3.8/site-packages/opensim-4.4.1-py3.8.egg/opensim/common.py", line 19551, in appendRow
    return _common.DataTableQuaternion_appendRow(self, *args)
TypeError: Wrong number or type of arguments for overloaded function 'DataTableQuaternion_appendRow'.
  Possible C/C++ prototypes are:
    OpenSim::DataTable_< double,SimTK::Quaternion_< double > >::appendRow(double const &,std::initializer_list< SimTK::Quaternion_< double > > const &)
    OpenSim::DataTable_< double,SimTK::Quaternion_< double > >::appendRow(double const &,OpenSim::DataTable_< double,SimTK::Quaternion_< double > >::RowVector const &)
    OpenSim::DataTable_< double,SimTK::Quaternion_< double > >::appendRow(double const &,OpenSim::DataTable_< double,SimTK::Quaternion_< double > >::RowVectorView const &)

I've tried creating a _DataTableQuaternion_ but got the same error:

Code: Select all

import numpy as np
import opensim as osim

SAMPLE_DATA = np.array(
    [
        [0, 0.9995429673603463, -0.008673246070566128, 0.02895804025163351, -0.0002512127741937707, 0.9988056889348111,
         0.009225910844409206, 0.04797793296728128, 0.0004430222738762587],
        [0.01, 0.9983235155232013, -0.01648178176931649, 0.0554767580235808, -0.0009157195522055893, 0.9953451329047127,
         0.01842024119373089, 0.0945817043153574, 0.001749950304193833],
        [0.02, 0.996553572768251, -0.02364517310391066, 0.0794879005037651, -0.001885753224544679, 0.9898697043858768,
         0.0275592750627042, 0.1392244052239398, 0.003875524103277203],
        [0.03, 0.9944026364977537, -0.03036249384785823, 0.1011532414540192, -0.003088241732745588, 0.9826840433510849,
         0.0366053997686734, 0.1815114641069645, 0.006760476956889426],
        [0.04, 0.9920155995198686, -0.03624118422120523, 0.1207152892736455, -0.004409740475433332, 0.974111394130037,
         0.04551417126443761, 0.22119823325065, 0.01033410116740631],
        [0.05, 0.9894759113314432, -0.04174418491853947, 0.1384223432967164, -0.005839417483162094, 0.9644686836064071,
         0.05440249635598614, 0.2581251100202142, 0.01455865068655974],
        [0.06, 0.9868508854848144, -0.04693285443957362, 0.154494844744801, -0.007347104141869534, 0.9540736552868223,
         0.06326532185989953, 0.2921740174350191, 0.01937273531245459],
        [0.07, 0.9842021159098119, -0.05180189751605759, 0.1690667958671119, -0.00889814517826073, 0.943280037377913,
         0.0721245581574768, 0.3231261113774534, 0.0247049659177489],
        [0.08, 0.9816277378550423, -0.05621921067018604, 0.1820376363912426, -0.01042514087690647, 0.9322749346658372,
         0.08098224817958229, 0.3512470753675765, 0.03050923990061744],
        [0.09, 0.9791306321049124, -0.06029023660482678, 0.1937163440868847, -0.01192772725965339, 0.9212212941768754,
         0.09000505440402377, 0.3766909623592524, 0.03680130722775423],
        [0.10, 0.9767195496681569, -0.06392963022845036, 0.2043356497812238, -0.01337407579504081, 0.9103290086705185,
         0.09894767715988709, 0.399530682092155, 0.04342450028027244]
    ]
)

time = SAMPLE_DATA[:, 0]
thorax_data = SAMPLE_DATA[:, 1:5]
humerus_data = SAMPLE_DATA[:, 5:]

dtable = osim.DataTableQuaternion()

for t, thorax, humerus in zip(time, thorax_data, humerus_data):
    dtable.appendRow(t, np.array([thorax, humerus]))
    
"""
Traceback (most recent call last):
  File "<stdin>", line 2, in <module>
  File "/home/pps4/.pyenv/versions/stv/lib/python3.8/site-packages/opensim-4.4.1-py3.8.egg/opensim/common.py", line 19551, in appendRow
    return _common.DataTableQuaternion_appendRow(self, *args)
TypeError: Wrong number or type of arguments for overloaded function 'DataTableQuaternion_appendRow'.
  Possible C/C++ prototypes are:
    OpenSim::DataTable_< double,SimTK::Quaternion_< double > >::appendRow(double const &,std::initializer_list< SimTK::Quaternion_< double > > const &)
    OpenSim::DataTable_< double,SimTK::Quaternion_< double > >::appendRow(double const &,OpenSim::DataTable_< double,SimTK::Quaternion_< double > >::RowVector const &)
    OpenSim::DataTable_< double,SimTK::Quaternion_< double > >::appendRow(double const &,OpenSim::DataTable_< double,SimTK::Quaternion_< double > >::RowVectorView const &)
"""

Using a _list_ instead of a _numpy.array_ returned the same error. I've then tried creating a _RowVector_ (as indicated by the error) but I don't see any method that allows me to fill it with my data:

Code: Select all

rvector = osim.RowVectorQuaternion()
dir(rvector)
"""
['CppNScalarsPerElement', 'NScalarsPerElement', '__call__', '__class__', '__delattr__', '__dict__', '__dir__', '__doc__',
'__eq__', '__format__', '__ge__', '__getattribute__', '__gt__', '__hash__', '__init__', '__init_subclass__', '__le__', '__lt__',
'__module__', '__ne__', '__new__', '__pos__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__',
'__subclasshook__', '__swig_destroy__', '__weakref__', 'begin', 'clear', 'col', 'end', 'getElt', 'index', 'isResizeable', 'lockShape',
'ncol', 'negateInPlace', 'nelt', 'nrow', 'resize', 'resizeKeep', 'row', 'setTo', 'setToNaN', 'setToZero', 'size', 'sum', 'this', 'thisown',
'unlockShape', 'updCol', 'updElt', 'updIndex', 'updRow']
"""

Seeing the constructor for _RowVectorQuaternion_ I understand that I can create an instance with N quaternions Q, but I can't see how I can create an instance with different quaternions Q1, Q2, Q3, ...., QN as to pass it to _DataTableQuaternion_ or _TimeSeriesTableQuaternion_ later.

Code: Select all

osim.RowVectorQuaternion("a")

"""
Traceback (most recent call last):
  File "<stdin>", line 1, in <module>
  File "/home/pps4/.pyenv/versions/stv/lib/python3.8/site-packages/opensim-4.4.1-py3.8.egg/opensim/simbody.py", line 4600, in __init__
    _simbody.RowVectorQuaternion_swiginit(self, _simbody.new_RowVectorQuaternion(*args))
TypeError: Wrong number or type of arguments for overloaded function 'new_RowVectorQuaternion'.
  Possible C/C++ prototypes are:
    SimTK::RowVector_< SimTK::Quaternion_< double > >::RowVector_()
    SimTK::RowVector_< SimTK::Quaternion_< double > >::RowVector_(SimTK::RowVector_< SimTK::Quaternion_< double > > const &)
    SimTK::RowVector_< SimTK::Quaternion_< double > >::RowVector_(SimTK::RowVectorBase< SimTK::Quaternion_< double > > const &)
    SimTK::RowVector_< SimTK::Quaternion_< double > >::RowVector_(int)
    SimTK::RowVector_< SimTK::Quaternion_< double > >::RowVector_(int,SimTK::Quaternion_< double > const *)
    SimTK::RowVector_< SimTK::Quaternion_< double > >::RowVector_(int,SimTK::Quaternion_< double > const &)
"""

What am I doing wrong? Any pointers are appreciated.

Thank you!

[Nicholas Bianco]

Hi Arthur,

You've almost got it. Here's what you need to create your table:

Code: Select all

# Create an empty table.
qtable = osim.TimeSeriesTableQuaternion()

# At each time point in the data set, we need to:
#    1. Construct Quaternions for the thorax and humerus
#    2. Construct two element RowVector containings these Quaternions
#    3. Use the RowVector and time point to append the row to the table 
for t, thorax, humerus in zip(time, thorax_data, humerus_data):

    # Construct Quaternions
    qthorax = osim.Quaternion(thorax[0], thorax[1], thorax[2], thorax[3])
    qhumerus = osim.Quaternion(humerus[0], humerus[1], humerus[2], humerus[3])
    
    # Construct RowVectorQuaternion
    qRowVec = osim.RowVectorQuaternion(2)
    qRowVec[0] = qthorax
    qRowVec[1] = qhumerus

    # Append the row to the table 
    qtable.appendRow(t, qRowVec)
    
# Now that we've created the table, set the column labels.
labels = osim.StdVectorString()
labels.append('q_thorax')
labels.append('q_humerus')
qtable.setColumnLabels(labels)

# This might also work
# qtable.setColumnLabels(['q_thorax', 'q_humerus'])

[Arthur Matta]

Hi Nicholas,

Thank you, I'll adapt the code and give you some feedback.

[Arthur Matta]

Hi,

Following-up my last question.

When doing

Code: Select all

qrowvec = osim.RowVectorQuaternion(2)
qrowvec[0] = osim.Quaternion(...)

I get the following error

Code: Select all

TypeError: 'RowVectorQuaternion' object does not support item assignment

Also, my goal is to simulate an individual movement in real time using data collected from several IMU sensors. Given the following code in which I recreate my TimeSeriesTableQuaternion (TSTQ) with the new data and feed it to the InverseKinematicsSolver (IKS):

Code: Select all

model = osim.Model("path_to_model.osim")
state = model.initSystem()

while True:
	# Get data from IMUs and create qtable as described by Nicholas.
	qtable = ...
	
	# Rotate qtable according to mapping from IMU to OpenSim orientation
	osim.OpenSenseUtilities.rotateOrientationTable(qtable, osim.Rotation(...))

	ik_solver = osim.InverseKinematicsSolver(
		model,
		osim.MarkersReference(),
		osim.OrientationsReference(osim.OpenSenseUtilities.convertQuaternionsToRotations(qtable)),
		osim.SimTKArrayCoordinateReference()
	)
	
	state.setTime(qtable.getIndependentColumn()[0])
	ik_solver.assemble(state)
	
	for i, t in enumerate(qtable.getIndependentColumn()):
		state.setTime(t)
		ik_solver.track(state)
		
		...
		
		# Read CoordinateSet to get the desired angles
		value = model.getCoordinateSet().get(coord).getValue(state) * RAD2DEG
		
		...
		

How can I update the IKS with the new TSTQ so that I don't need to keep recreating and assembling it? I'd would like to do something like this:

Code: Select all

model = osim.Model("path_to_model.osim")
state = model.initSystem()

# Set t=0
state.setTime(0.0)

# Create empty TSTQ
qtable = TimeSeriesTableQuaternion()

# Create initial IKS
ik_solver = osim.InverseKinematicsSolver(
	model,
	osim.MarkersReference(),
	osim.OrientationsReference(osim.OpenSenseUtilities.convertQuaternionsToRotations(qtable)),
	osim.SimTKArrayCoordinateReference()
)

# Assemble IKS with initial state
ik_solver.assemble(state)

while True:
	# Recreate TSTQ with new data
	qtable = ...
	
	# Rotate qtable according to mapping from IMU to OpenSim orientation
	osim.OpenSenseUtilities.rotateOrientationTable(qtable, osim.Rotation(...))

	# Update IKS with new data
	ik_solver.update_orientations(qtable)
	
	# Update state according to the independent column from the new qtable and get the angle values I need.
	for i, t in enumerate(qtable.getIndependentColumn()):
		state.setTime(t)
		ik_solver.track(state)
		
		...
		
		# Read CoordinateSet to get the desired angles
		value = model.getCoordinateSet().get(coord).getValue(state) * RAD2DEG
		
		...
		

I see that there are two methods, updateOrientationWeight and updateOrientationWeights, that might do what I want but I'm having trouble figuring out how to use them.

Thank you for the support,
Arthur Matta

[Arthur Matta]

Hello,

any update to my last question?

Best regards,
Arthur Matta

[Thomas Uchida]

Code: Select all

qrowvec = osim.RowVectorQuaternion(2)
#instead of
qrowvec[0] = osim.Quaternion(...)
#perhaps try
qrowvec.set(0, ...)

https://simtk.org/api_docs/simbody/3.5/ ... b3275b4802

[Arthur Matta]

Hello @Thomas,

I've tried your suggestion. Unfortunately, it didn't work

Code: Select all

import opensim as osim
qrowvec = osim.RowVectorQuaternion(2)
qrowvec.set(0, osim.Quaternion(1,0,0,0))

"""
Traceback (most recent call last):
  File "<stdin>", line 1, in <module>
AttributeError: 'RowVectorQuaternion' object has no attribute 'set'
"""

Also, any suggestions to the InverseKinematicsSolver problem?

Best regards,
Arthur Matta

[Adam Yoder]

Not convenient, but updElt() method of RowVectorQuaternion works, rather than set() or qRowVec[#] = q
Was useful to verify functionality of setRotationReference() in MocoOrientationTrackingGoal, which accepts tables of Quaternion or Rotation (after a fix in v4.4. GitHub Issue)

Code: Select all

% MATLAB
q = Quaternion(); %could also come from a file 
q.setToZero();

quatTable = TimeSeriesTableQuaternion();
qLabels = StdVectorString();
qLabels.add('/bodyset/pelvis')
qLabels.add('/bodyset/calcn_r');
qLabels.add('/bodyset/calcn_l');
quatTable.setColumnLabels(qLabels)

for i=(1:numel(time))-1
    qRowVec = RowVectorQuaternion(qLabels.size());
    for ii=(1:qLabels.size())-1
        q0=qRowVec.updElt(0,ii);
        q0.set(0,q(1))
        q0.set(1,q(2))
        q0.set(2,q(3))
        q0.set(3,q(4))
    end
    quatTable.appendRow(i,qRowVec)
    quatTable.setIndependentValueAtIndex(i,t(i+1))
end

STOFileAdapterQuaternion.write(quatTable,'tracking_quaternions.sto')