'InverseKinematicsSolver' object has no attribute 'addOrientationValuesToTrack'

[flamerten]

I am currently attempting to recreate the OpenSenseRT. I have successfully flashed a microsd card with Ubuntu Server 20.04, built opensim on Linux and prepared it for scripting in python.

However, while following the file ik_streaming.py, I got stuck, and was unable to run the full program due to the error as written in the title.

Starting recording...
Traceback (most recent call last):
  File "ik_streaming.py", line 153, in <module>
    ikSolver.addOrientationValuesToTrack(time_s+dt, rowVec)
AttributeError: 'InverseKinematicsSolver' object has no attribute 'addOrientationValuesToTrack

I'm not too sure why Im facing such an error. From looking through the documentation, I do not find any methods that mention addOrientationValuesToTrack. However, OpenSenseRT is based on a research paper and its weird there is a function that does not exist. Are there other alternatives to that line which I could consider to continue on with the OpenSenseRT code?

[aymanhab]

The API referred to here is obsolete and was only used during the development of 4.3 but then got subsumed by more recent versions. Please point to the code base using this interface so we can upgrade or use it with pre-built version of the libraries consistent with this API.

[flamerten]

Hi @aymanhab thanks for the reply, this makes sense.

The code base that was using it was the Github Repo of OpensenseRT
https://github.com/pslade2/RealTimeKin/blob/94b6f6dda6d369ea565517c2878d1c164f5424ab/ik_streaming.py#L150

By any chance, would you know how i could replace that line? Thank you!

[flamerten]

@aymanhab I would like to provide an update.

It seems as though that the addOrientationsToTrack method has been replaced by the BufferedOrientationsReference object, where you keep a buffer to store data. From there, I modified the code based on 2 Pull Requests i found: #2843 and #2855. #2843 uses addOrientationsToTrack but #2855 updated this with certain changes. However, my attempts to replicate this have led to 2 errors.

Error 1 - BufferedOrientationsReference object Declaration

quatTable = osim.TimeSeriesTableQuaternion(sto_filename)
orientationsData = osim.OpenSenseUtilities.convertQuaternionsToRotations(quatTable)
oRefs = osim.BufferedOrientationsReference(orientationsData) #error

TypeError: Wrong number or type of arguments for overloaded function 'new_BufferedOrientationsReference'.
  Possible C/C++ prototypes are:
    OpenSim::BufferedOrientationsReference::BufferedOrientationsReference()
    OpenSim::BufferedOrientationsReference::BufferedOrientationsReference(OpenSim::BufferedOrientationsReference const &)
    OpenSim::BufferedOrientationsReference::BufferedOrientationsReference(OpenSim::BufferedOrientationsReference &&)

I am not sure why orientationsData cannot be passed in as a parameter, as BufferedOrientationsReference is a subclass of OrientationsReference.

Error 2 - putValues method

quatTable = osim.TimeSeriesTableQuaternion(sto_filename)
orientationsData = osim.OpenSenseUtilities.convertQuaternionsToRotations(quatTable)
rowVecView = orientationsData.getNearestRow(time_s)
rowVec = osim.RowVectorRotation(rowVecView)


oRefs.putValues(time_s+dt, rowVecView)

Traceback (most recent call last):
  File "ik_streaming.py", line 183, in <module>
    oRefs.putValues(time_s+dt, rowVec)
  File "/home/ubuntu/opensim-core/sdk/Python/opensim/simulation.py", line 34068, in putValues
    return _simulation.BufferedOrientationsReference_putValues(self, time, dataRow)
TypeError: in method 'BufferedOrientationsReference_putValues', argument 3 of type 'SimTK::RowVector_< SimTK::Rotation > const &'

I find this strange as well, because there was a very similar line that was one in the testLiveIK.cpp file, in this line here. From what i understand, the reason why this error is thrown is due to getNearestRow returning a rowVectorView object. However, the putValues method wants a rowVector object. Again, this is strange because rowVectorView in a subclass of rowVector so it should be accepted as a rowVector.

I'm not sure the reason for these errors. After debugging, I have a feeling there might be an issue with the python build, perhaps the SWIG script had some errors leading to some public methods not being exported when the python version was built.

Hope you can provide some assistance by clarifying if there is indeed a bug with SWIG, a problem with my python build or a misunderstanding of the C++ API. Thank you.

[RTnhN]

This is still a problem. I have tried changing to Swig version 4.0.2 to see if it is a problem with the newest version. I also noticed that the real time part of opensense (specifically the bufferedOrientationReference part) is not part of the test suite.

[RTnhN]

I found the problem. Evidentially there were some missing types. While it might have worked for c++, I think that it confused Swig. I fixed it here: b6862e9. I will plan on making a PR this week. I might add some tests related to this when building so that we can catch some of these problems in the future.

[nickbianco]

Good catch @RTnhN! Happy to review your PR whenever it comes in.

[RTnhN]

Thanks, @nickbianco! The PR is #3644.

[RTnhN]

We should be able to close this issue now that the PR is merged, right?

[aymanhab]

Sure, closing!

[RTnhN]

I realized that this is not completely solved. SWIG has a problem with initializing constructors from base classes using using super::super;. This was a problem until swig 4.2.0 which was released recently. I need the constructor of OrientationReference for BufferedOrienationReference to add the Names for the different orientations and initialize it correctly. I thought that just using the method putValues could be used for initialization, but it does not initialize it properly as seen in pslade2/RealTimeKin#8.

You can see where they fixed the problem here: https://www.swig.org/Doc4.2/CPlusPlus11.html#CPlusPlus11_object_construction_improvement (notice that they say that using the using keyword is not possible with earlier versions of SWIG)

So I think the solution would be to change to SWIG 4.2.0, but this is kind of a headache. I have done a little work with it, and it looks like they made a bunch of changes, so it is not as easy as changing all references of SWIG 4.1.1 to 4.2.0. For example, they removed an interface file std_carray.i and merged it into std_array.i, so where we reference it below will need to be changed.

opensim-core/Bindings/Python/swig/python_preliminaries.i

Line 15 in 4c76879

%include "std_carray.i";

[aymanhab]

@RTnhN Thanks for reporting, with that should this issue be reopened? Also did the previous commit make any difference to justify patching into 4.5 release as it stands? Thank you

[RTnhN]

@aymanhab Maybe it might be good to open up another issue and reference this one. This issue had a bunch of different problems, some of which were resolved.

The previous commit does make a difference. It resolved one of the problems in this issue, so I think it justifies patching into 4.5.

[RTnhN]

Acutally, I think it would be good to open it again.

[RTnhN]

I tried manually changing the using statement to the explicit declaration, and this compiled well and the bindings were able to be made and used. I know that this is kind of ugly and not ideal, but the current version of SWIG does not allow using the using statement for constructors and updating SWIG is a major headache. It has been breaking lots of builds.

     // CONSTRUCTION
     //--------------------------------------------------------------------------
     BufferedOrientationsReference();
+    // Constructor with orientation file name and model units
+    BufferedOrientationsReference(const std::string& orientationFileName,
+                                  Units modelUnits=Units(Units::Radians))
+        : OrientationsReference(orientationFileName, modelUnits) {}
+    // Constructor with TimeSeriesTable and optional OrientationWeightSet
+    BufferedOrientationsReference(const TimeSeriesTable_<SimTK::Rotation_<double>>& orientationData,
+                                  const Set<OrientationWeight>* orientationWeightSet=nullptr)
+        : OrientationsReference(orientationData, orientationWeightSet) {}
     BufferedOrientationsReference(
             const BufferedOrientationsReference&) = default;
     BufferedOrientationsReference(BufferedOrientationsReference&&) = default;
     BufferedOrientationsReference& operator=(
             const BufferedOrientationsReference&) = default;
 
-    // Use OrientationsReference convenience costructor from TimeSeriesTable
-    using OrientationsReference::OrientationsReference;
 
     virtual ~BufferedOrientationsReference() {}

Using this as a reference, I made some tweaks to Patrick's code, and while I was able to create a copy of BufferedOrientationsReference with a data table so that I could get the names of the segments, I ran into a problem.

When I ran approximately these commands, I got the following runtime error:

quatTable = osim.TimeSeriesTableQuaternion(sto_filename)
orientationsData = osim.OpenSenseUtilities.convertQuaternionsToRotations(quatTable)
oRefs = osim.BufferedOrientationsReference(orientationsData)
oRefs.setDefaultWeight(1.0)
init_state = model.initSystem()
mRefs = osim.MarkersReference()
coordinateReferences = osim.SimTKArrayCoordinateReference()
model.initSystem()
s0 = init_state
ikSolver = osim.InverseKinematicsSolver(model, mRefs, oRefs, coordinateReferences, constraint_var)
ikSolver.setAccuracy = accuracy
s0.setTime(0.)
ikSolver.setAdvanceTimeFromReference(True)
ikSolver.assemble(s0)

  File "/home/sage/RealTimeKin/ik_streaming.py", line 117, in <module>
    ikSolver.assemble(s0)
  File "/home/sage/.local/lib/python3.7/site-packages/opensim/simulation.py", line 33344, in assemble
    return _simulation.AssemblySolver_assemble(self, s)
RuntimeError: std::exception in 'void OpenSim::AssemblySolver::assemble(SimTK::State &)': getNextValuesAndTime method is not supported for this reference {}.

This is exactly what is expected if I was using the "OrientationsReference" object in the inverseKinematicSolver.

opensim-core/OpenSim/Simulation/OrientationsReference.h

Lines 129 to 134 in 98c70b1

	virtual double getNextValuesAndTime(
	SimTK::Array_<SimTK::Rotation_<double>>& values) override {
	throw Exception("getNextValuesAndTime method is not supported for this "
	"reference {}.",
	this->getName());
	};

I was explicitly not using the base OrientationsReference though. In fact, if I manually call the getNextValuesAndTime method on the oRef variable, it returns the expected information. So for some reason, the InverseKinematicsSolver object is using the base (super) method instead of the derived (self) method. I see that InverseKinematicsSolver constructor is defined with the std::shared_ptr<OrientationsReference> which hopefully won't cause the InverseKinematicsSolver to use the base class method instead of the derived class method. We don't need to create a constructor for the InverseKinematicSolver that explicitly uses like a shared pointer to BufferedOrientationsReference, right?

As a sanity check, the getNextValuesAndTime method is declared as virtual in the base class, it has the same signatures in both the base class and the derived class, and it has the override keyword even though this is more optional than a strict requirement. I don't get any squawks from the compiler about not having a corresponding virtual method, so it seems that the compiler associates these two methods (the method in the derived class and the method in the base class) well.

I also tried not using "setAdvanceTimeFromReference" and manually adjusting the time, but that just gave me a "key not found" error which is what is expected if it was calling the base method and was trying to find a value in the table just used to construct the BufferedOrienationsReference.

Do you have any idea why the inheritance is not working as expected?

[aymanhab]

Thanks for investigating this, I'd try a constructor for the solver that takes the shared_ptr to BufferedOrientationsReference to test if this is a swig issue or something else as handling of smart pointers across the interface is rather fragile. Let me know what you find out. Thank you

[RTnhN]

That's a great point. I can try that and see if that fixes it. Thanks for the idea.

[RTnhN]

That does fix the problem. I found out that I also needed to update the swig interface file to allow both shared pointers and regular pointers like what is done with the MarkerReference and the regular OrientationReference.

opensim-core/Bindings/simulation.i

Lines 224 to 240 in 120db03

	%include <OpenSim/Simulation/MarkersReference.h>
	// Since we have both MarkersReference and shared_ptr<MarkersReference>
	// Across the interface, DO NOT use %shared_ptr macro here as it treats
	// all pointers and references as std::shared_ptr
	//
	%template(SharedMarkersReference) std::shared_ptr<OpenSim::MarkersReference>;
	%template(SetMarkerWeights) OpenSim::Set<MarkerWeight, OpenSim::Object>;
	%include <OpenSim/Simulation/CoordinateReference.h>
	%include <OpenSim/Simulation/OrientationsReference.h>
	// Since we have both OrientationsReference and shared_ptr<OrientationsReference>
	// Across the interface, DO NOT use %shared_ptr macro here as it treats
	// all pointers and references as std::shared_ptr
	//
	%template (SetOientationWeights) OpenSim::Set<OrientationWeight, OpenSim::Object>;
	%template(SharedOrientationsReference) std::shared_ptr<OpenSim::OrientationsReference>;
	%include <OpenSim/Simulation/BufferedOrientationsReference.h>
	%shared_ptr(OpenSim::BufferedOrientationsReference);

The problem that I have now is that the ikSolver gets hung whenever I use the track method. It assembles fine, but it might not be converging. I have tried with the sample data in the sample code for OpenSim in case it is just a case of bad data, but data that does converge when using the IMUInverseKinematicsTool does not converge (or has other problems) when manually implementing the ik with the InverseKinematicsSolver class. I'll get back with what I find out.

Edit: I wanted to recompile the project without the java bindings and in the RelwithDebInfo in case I needed to directly debug the c++ code, so I did a totally clean build. This gave me a chance to test the regular OrientationsReference. Using the following code after running the OpenSense_IMUDataConverter.py and OpenSense_CalibrateModel.py in the Resources/Code/Python/OpenSenseExamples folder in the build folder, I was able to successfully track:

import opensim as osim
from math import pi
# Set variables to use
modelFileName = 'calibrated_Rajagopal_2015.osim';                # The path to an input model
orientationsFileName = 'MT_012005D6_009-001_orientations.sto';   # The path to orientation data for calibration 
model = osim.Model(modelFileName)
quatTable = osim.TimeSeriesTableQuaternion(orientationsFileName)
orientationsData = osim.OpenSenseUtilities.convertQuaternionsToRotations(quatTable)
oRefs = osim.BufferedOrientationsReference(orientationsData)
init_state = model.initSystem()
mRefs = osim.MarkersReference()
coordinateReferences = osim.SimTKArrayCoordinateReference()
model.initSystem()
s0 = init_state
ikSolver = osim.InverseKinematicsSolver(model, mRefs, oRefs, coordinateReferences, 0.1)
s0.setTime(0.)
ikSolver.setAccuracy(0.1)
ikSolver.assemble(s0)
for t in oRefs.getTimes():
  s0.setTime(t)
  ikSolver.track(s0)

I'm going to go back and add the BufferedOrientaitonsReference changes back and see if it will work with the values being added with the putValues method.

Edit again:

I am going though adding logging to see where it is getting hung. So far, it seems that there might be a problem with the data queue.

[RTnhN]

Okay, I added the right constructor and it works somewhat. I can put data into the data queue for the bufferedOrientationsReference, but it still hangs at the ikSolver.track(s0) step. I found out that the oRefs object in python and the oRefs object in the ikSolver object are different. I put logging in the queue to tell me when data is pushed or popped. I push data and it says that it is pushed. I go to pop the data, but there is no data to pop. I use the python method to get data, and it says that it was popped. It does not say that it was popped any other time, so I think they are two different objects: the oRefs = osim.BufferedOrientationsReference(orientationsData) object and the object referenced in the ikSolver object. Overall, I think this is probably a smart pointer problem.

When digging into how Swig deals with smart pointers, I did see some things that might need to change in the interface files for it to work.

This is all from the SWIG documentation on smart pointers

1. Strangely enough, the definition needs to happen after the shared pointer macro is used. So, it should be:

%shared_ptr(BufferedOrientationsReference);
%include <OpenSim/Simulation/BufferedOrientationsReference.h>

"Note that the %shared_ptr(IntValue) declaration occurs after the inclusion of the boost_shared_ptr.i library which provides the macro and, very importantly, before any usage or declaration of the type, IntValue."

2. This is a pretty large change, but I think that every base class of a derived class needs to be defined as a smart pointer using the macro.

"A proxy class derived from a base which is being wrapped with shared_ptr can and must be wrapped as a shared_ptr too. In other words all classes in an inheritance hierarchy must all be used with the %shared_ptr macro."

When I compiled with the right order (macro before definition), it gave me the expected warnings:

/home/sage/opensim-workspace/opensim-core-source/OpenSim/Simulation/BufferedOrientationsReference.h:44: Warning 520: Base class 'OpenSim::OrientationsReference' of 'BufferedOrientationsReference' is not similarly marked as a smart pointer.
/home/sage/opensim-workspace/opensim-core-source/OpenSim/Simulation/BufferedOrientationsReference.h:44: Warning 520: Base class 'OpenSim::StreamableReference_< SimTK::Rotation_< double > >' of 'BufferedOrientationsReference' is not similarly marked as a smart pointer.
/home/sage/opensim-workspace/opensim-core-source/OpenSim/Simulation/BufferedOrientationsReference.h:44: Warning 520: Base class 'OpenSim::Reference_< SimTK::Rotation_< double > >' of 'BufferedOrientationsReference' is not similarly marked as a smart pointer.
/home/sage/opensim-workspace/opensim-core-source/OpenSim/Simulation/BufferedOrientationsReference.h:44: Warning 520: Base class 'OpenSim::Object' of 'BufferedOrientationsReference' is not similarly marked as a smart pointer.

The macro is seemingly used correctly in most of the repo except for below where they should be moved up:

opensim-core/Bindings/simulation.i

Lines 213 to 223 in f291e0b

	%template(ReferenceVec3) OpenSim::Reference_<SimTK::Vec3>;
	%template(ReferenceDouble) OpenSim::Reference_<double>;
	%template(ReferenceRotation) OpenSim::Reference_<SimTK::Rotation_<double>>;
	%template(StreamableReferenceRotation) OpenSim::StreamableReference_<SimTK::Rotation_<double>>;
	%template(SimTKArrayCoordinateReference) SimTK::Array_<OpenSim::CoordinateReference>;
	%shared_ptr(ReferenceVec3);
	%shared_ptr(ReferenceDouble);
	%shared_ptr(ReferenceRotation);

Actually, when I rearrange it, I get this error:

/home/sage/opensim-workspace/opensim-core-build/Bindings/Python/python_simulation_wrap.cxx: In function ‘PyObject* _wrap_delete_OrientationsReference(PyObject*, PyObject*)’:
/home/sage/opensim-workspace/opensim-core-build/Bindings/Python/python_simulation_wrap.cxx:329637:26: error: ‘smartarg1’ was not declared in this scope
       (void)arg1; delete smartarg1;
                          ^~~~~~~~~
/home/sage/opensim-workspace/opensim-core-build/Bindings/Python/python_simulation_wrap.cxx:329637:26: note: suggested alternative: ‘slartg_’
       (void)arg1; delete smartarg1;
                          ^~~~~~~~~
                          slartg_
/home/sage/opensim-workspace/opensim-core-build/Bindings/Python/python_simulation_wrap.cxx: In function ‘PyObject* _wrap_delete_BufferedOrientationsReference(PyObject*, PyObject*)’:
/home/sage/opensim-workspace/opensim-core-build/Bindings/Python/python_simulation_wrap.cxx:332713:26: error: ‘smartarg1’ was not declared in this scope
       (void)arg1; delete smartarg1;
                          ^~~~~~~~~
/home/sage/opensim-workspace/opensim-core-build/Bindings/Python/python_simulation_wrap.cxx:332713:26: note: suggested alternative: ‘slartg_’
       (void)arg1; delete smartarg1;
                          ^~~~~~~~~
                          slartg_
ninja: build stopped: subcommand failed.

When I researched it, it turned out to be the problem of not having all of the base classes defined as smart pointers based off of this stack overflow question. I think the solution to this will be a lot harder than expected. I am still trying things, but I just wanted to get an update out.

[RTnhN]

I don't think that using smart pointers will be a viable solution since the swig support for them is lacking and is causing major problems.

It seems that the only way to get real-time inverse kinematics going is to do something similar to how the iksolver did real time in the past. This involves using a method connected to the iksolver object rather than the directly on the orientaionsReference object. I have tested the following addition into the InverseKinematicsSolver class, and it is able to run the RealTimeKin code.

void InverseKinematicsSolver::updateOrientationData(double time, const SimTK::RowVectorView_<SimTK::Rotation_<double>>& dataRow)
{   
_orientationsReference->putValues(time, dataRow); 
}

What do you think are next steps? I could just use my fork of the repo for my work, but I would rather find a workable solution for the main repo.

[aymanhab]

Thanks for your work on this @RTnhN much appreciated 👍

It would be good to take a look before lifting the API to the solver (because this type of call is supported only for the special BufferedOrientationReference and not any other Reference type so could be confusing to users). I totally agree that SWIG handling of shared_ptr is poorly documented, but hopefully we can figure out the issue/learn something from it for wider use before changing the API on the c++ side. Maybe a PR wit the problem code would help. Thanks again

[RTnhN]

Okay, I created a PR so that you can see the code so far. I did find out that the namespace was missing. Once I added it, I did not have any problems with it compiling, but it still does not work. I put data in the queue, but the queue is empty when I go to get data.