Posing a Model from coordinates in IK motion file

[Colin Smith]

Hi All,

No need to respond to this, just wanted to repost here so that the answers are searchable for others with similar questions.

I am trying to read in the results from an IK file, then set the coordinates and generalized speeds in the model. The issue I am having is that I have the IK results in a SimTK::Matrix and I need to set the Q and U using a SimTK::Vector. This means I have to get a row from the matrix and convert it to a SimTK::Vector. My code throws an error at the bold line below (state.setQ(q_matrix.getAsVector());) with the following error:

Assertion failed: ncol()==1, file c:\users\csmith\github\opensim\install\sdk\simbody\include\simtkcommon\internal\matrixbase.h, line 802

which I assume comes from the fact that q_matrix gives a SimTK::RowVectorView.
Any thoughts on how to get around this?

Also, is there an easy was to convert IK results that are "inDegrees" to radians? I would have used the StatesTrajectory here but it wouldn't let me read in a file that was "inDegrees". I don't see a way to check if a Coordinate is a rotation at the moment.

Lastly, is it safe to assume that the order of the coordinates that comes out of (const Coordinate& coord : model.getComponentList()) is the same order of the states.setQ() input?

Thanks,
Colin

int main(){
try {

//Load WISCO_Plugin
static const std::string plugin_file{ "../../../install/plugin/WISCO_Plugin_d" };
LoadOpenSimLibrary(plugin_file, true);


//Initialize Model
//----------------
static const std::string model_file = WISCO_HOME + "source/models/rajagopal/Scale/subject_scaled_walk.osim";
Model model(model_file);
model.setUseVisualizer(true);
SimTK::State state = model.initSystem();

//Load IK Motion File
//-------------------
static const std::string ik_motion_file = WISCO_HOME + "source/models/rajagopal/IK/results_walk/ik_output_walk.mot";
//StatesTrajectory ik_states;
//ik_states.createFromStatesStorage(model, ik_motion_file);
//Storage sto = Storage(ik_motion_file);
STOFileAdapter_<double> stofileadapter{};
auto ik_table = stofileadapter.read(ik_motion_file);

std::vector<double> time = ik_table.getIndependentColumn();
int nFrames = time.size();

//Find Coordinate and Speeds in Storage
SimTK::Matrix ik_matrix = ik_table.getMatrix();

SimTK::Matrix q_matrix(nFrames,model.getNumCoordinates());
SimTK::Matrix u_matrix(nFrames, model.getNumCoordinates());

int nCoord = 0;
for (const Coordinate& coord : model.getComponentList<Coordinate>()) {
std::string coord_name = coord.getName();
int coord_index = ik_table.getColumnIndex(coord_name);
GCVSpline q_spline;
q_spline.setDegree(5);

for (int i = 0; i < nFrames; ++i) {
q_spline.addPoint(time, ik_matrix(i, coord_index));
}
q_matrix(nCoord) = ik_matrix(coord_index);

for (int i = 0; i < nFrames; ++i) {
std::vector<int> order = { 0 };
SimTK::Vector x(1);
x(0)= time;
u_matrix(i, nCoord) = q_spline.calcDerivative(order, x);
}
nCoord++;


}

//Turn on Visualizer
SimTK::Visualizer::InputSilo* silo;
if (model.getUseVisualizer()) {
SimTK::Visualizer& viz = model.updVisualizer().updSimbodyVisualizer();
// We use the input silo to get key presses.
silo = &model.updVisualizer().updInputSilo();

SimTK::DecorativeText help("Press any key to start a new simulation; "
"ESC to quit.");
help.setIsScreenText(true);
viz.addDecoration(SimTK::MobilizedBodyIndex(0), SimTK::Vec3(0), help);

viz.setShowSimTime(true);
viz.drawFrameNow(state);
std::cout << "A visualizer window has opened." << std::endl;

// if visualizing enable replay
//simulateOnce = false;

// Get a key press.
silo->clear(); // Ignore any previous key presses.


}

//Loop Over Each Time Step
//------------------------
for (int i = 0; i < nFrames; ++i) {
std::cout << "Frame: " << i+1 << "/" << nFrames << std::endl;
std::cout << "Time: " << time << std::endl;
std::cout << "=====================================================\n" << std::endl;

//Pose the Model
state.setQ(q_matrix.getAsVector());
state.setU(u_matrix.getAsVector());
model.realizeVelocity(state);

unsigned key, modifiers;
silo->waitForKeyHit(key, modifiers);
if (key == SimTK::Visualizer::InputListener::KeyEsc) { break; }
}


// ********** END CODE **********
}
catch (OpenSim::Exception ex)
{
std::cout << ex.getMessage() << std::endl;
std::cin.get();
return 1;
}
catch (SimTK::Exception::Base ex)
{
std::cout << ex.getMessage() << std::endl;
std::cin.get();
return 1;
}
catch (std::exception ex)
{
std::cout << ex.what() << std::endl;
std::cin.get();
return 1;
}
catch (...)
{
std::cout << "UNRECOGNIZED EXCEPTION" << std::endl;
std::cin.get();
return 1;
}
std::cout << "OpenSim example completed successfully" << std::endl;
std::cout << "Press return to continue" << std::endl;
std::cin.get();
return 0;
}



Ajay's Response:

Some quick answers to get you rolling:

1) A Vector is a column vector so you need to take the transpose of a RowVector (e.g. try ~q_matrix, ~ is the transpose operator in Simbody).

2) If your values are in degrees then they are not state values. An IK motion file is not a states file. It only specifies coordinate values and not their speeds and neither are muscle states defined. We cannot call this a states trajectory if the states are unspecified. You can compose a state with coordinates alone by calling Model::formStateStorage but even here we assume coordinates as state values are in radians or meters.
You can call Coordinate::getMotionType() to determine if the coordinate is rotational (Coordinate::Rotational) or not.

3) It is not safe to assume the layout of generalized coordinates and their speeds in the State is conserved between instances nor that it matches with the order in which components are specified in the model. The order of generalized coordinates in the SimTK::State depends on the topology of the Multibody tree. If you add a joint or change its type, the topology can change and there is no guarantee the multibody tree will be the same or even similar, especially now that OpenSim uses the Simbody::MultibodyGraphMaker to construct an efficient/balanced tree automatically. Given the sense (parent to child) of a Joint specified by the user the MultibodyGraphMaker can automatically reverse a Joint to fundamentally change the tree. For example, if a segment is now welded to ground, it might determine that it is more efficient to build the Multibody tree from the ground up, since we can still maintain the sense of the Joint for the user (e.g. knee would still describe tibia motion in femur) if the joint is reversed. The only guarantee is that regardless off the topology under the covers and resulting State layout, the Model accessors of Model::g/setStateVariableValues() will maintain the same order as their names from Model::getStateVariableNames(). In fact, Model::formStateStorage uses these methods to ensure the values read in are attributed to the correct state variable entry.
Hope this helps.

[Colin Smith]

So just to clarify an issue I have had with SimTK::VectorView in C++ for a long time and now I think I finally understand:

If q_matrix is a SimTK::Matrix, then ~q_matrix will return a SimTK::VectorView.

In the visual studio IDE if you try to use the SimTK::VectorView in a function that is expecting a SimTK::Vector, the code "spell checker" will put a squiggly red line underneath as if there is an error. However, this actually builds and runs with no errors.

VectorView Error.JPG (22.92 KiB) Viewed 587 times

This is documented in the SimTK Simmatrix doc that comes with Simbody.

References of this type are called views since they provide alternate views of the same data. They retain all properties of the original object except that they cannot be resized. They are in fact represented identically to the original objects in the sense that they can be used wherever a Vector or Matrix reference is expected, without memory allocation or data copying.
The implementations of these types are opaque to a C++ program using them. That is, the header files define these as handle classes which contain only a pointer to an undefined type (essentially a void*). The object referenced is an instance of a hidden implementation class.* This permits use of these classes in SimTK interfaces while preserving binary compatibility. It also allows use of these objects from other languages, since the void* can serve as a lowest common denominator representation.

Also, thanks Ajay. Model::formStateStorage and Coordinate::getMotionType are exactly what I was looking for.

[Christopher Dembia]

Colin: Intellisense is separate from the Visual C++ compiler, so Intellisense sometimes generates an error when there is no actual error.

This is different from the warnings/errors generated in IDEs that use Clang or GCC: usually, these IDEs are actually running the compiler to generate the warnings/errors.