Kinematics

[Jean-Philippe Jodoin]

Hi, I'm trying to do Inverse Kinematics with SimTK and I would like to know the best way to achieve what I want. I have the start position of the bodies in the system (an arm for example) and I have the final position of the hand and I want to know the position and orientation of the joints. I have tried a couple of things. I have use some Constraint::PrescribedMotion with Linear Function (first coordinate is the start position and last position is the end position). I have also try setting the position with setQToFitTranslation after the realizeTopology and before the realize, but it doesn't seems to work. What would be the best way to achieve this ?

Regards,

Jean-Philippe

[Michael Sherman]

Hi, Jean-Philippe.

Have you tried the ObservedPointFitter? That's our generic inverse kinematics tool in Simbody -- you give it a bunch of spatial locations of stations (points on bodies) and it tries to find the internal coordinates that best-fit those points.

Regards,
Sherm

[Jean-Philippe Jodoin]

Wow ! Thanks a lot, this class seems to do more than we could have ever dreamed off ! There are a lot of hidden gems in SimTK ! We will try it and give you feedback. Thanks again !

Jean-Philippe

[Jean-Philippe Jodoin]

Hi, I've try the class and I like it . I have a problem tought. I get vector subscript out of range exception at line 252 of ObservedPointFitter.cpp
if (currentBodyIndex == (int) originalBodyIxs.size()-1 && stations[bodyIndex[id]].size() == 0. I have a system with a welded body with a 1 ball in socket, 1 hinges and another ball in socket (essentially, I'm doing a human arm). If I try to move the hand, or the forearm, every is ok. But if I try to move the arm or the body, I get the exception. I have added another hand with a ball and socket mobilizer and doing this does that I get the exception of the fore arm also. It seems like I can't go more than 2 mobilizedbody far from the the leaf of my "body tree" or I will get this exception.

Also, I was wondering, I would like to be able to contraint the mobilized body, for instance I would like to allow only 180 degree rotation on an hinges and things like that. You already told me to use force to accomplish this. I was wondering what would be the best way to do this with the Observed point fitter ?

Thanks a lot,

Jean-Philippe

[Michael Sherman]

Hi, Jean-Philippe.

I looked at the code and discussed it with Peter Eastman who wrote it. It appears that it may have a bug if any of the bodies has no points on it but it was hard to tell. If you can, please send some code we can use to reproduce it here.

ObservedPointFitter does not support joint limits (although that would be a nice feature!). I think the best way to accomplish that now would be to add more points or fiddle with the weights so that the solution it finds is more reasonable. Otherwise you may have to polish up the solution afterwards yourself.

Regards, Sherm

[Jean-Philippe Jodoin]

Hi Sherm, thanks a lot for the support. I have reproduce the bug by doing this:

#include "SimTKsimbody.h" //SimTK principal Library

using namespace SimTK;

int main()
{

MultibodySystem system;
SimbodyMatterSubsystem matter(system); //Subsystem that contains all the bodies of the system
GeneralForceSubsystem forces(system); //Subsystem that contains all the forces of the system


Mat33 inertiaMatrix(1,0,0,0,1,0,0,0,1);
Vec3 centerOfMass(0);

//Body Creations
Body::Rigid body(MassProperties(120, centerOfMass, Inertia(inertiaMatrix)));
Body::Rigid arm(MassProperties(15, centerOfMass, Inertia(inertiaMatrix)));
Body::Rigid foreArm(MassProperties(15, centerOfMass, Inertia(inertiaMatrix)));
Body::Rigid hand(MassProperties(15, centerOfMass, Inertia(inertiaMatrix)));

MobilizedBody::Weld attach(matter.Ground(), Transform(Vec3(0,0,0)), body, Transform(Vec3(0,0,0)));
MobilizedBody::Ball shoulder(attach, Transform(Vec3(2,3,0)), arm, Transform(Vec3(0,0,0)));
MobilizedBody::Ball elbow(shoulder, Transform(Vec3(2,0,0)), foreArm, Transform(Vec3(0,0,0)));
MobilizedBody::Ball wrist(elbow, Transform(Vec3(2,0,0)), hand, Transform(Vec3(0,0,0)));
MobilizedBody::Ball wrist2(wrist, Transform(Vec3(2,0,0)), hand, Transform(Vec3(0,0,0)));

try
{
// Initialize the system and state.
system.realizeTopology();
State state = system.getDefaultState();
std::vector<MobilizedBodyIndex> indexVector;
std::vector<std::vector<Vec3>> originPositionVector;
std::vector<std::vector<Vec3>> targetPositionVector;

std::vector<Vec3> originPositionOfBody;
originPositionOfBody.push_back(Vec3(0));

std::vector<Vec3> targetPositionOfBody;
targetPositionOfBody.push_back(Vec3(2,7,0));

indexVector.push_back(elbow.getMobilizedBodyIndex());
originPositionVector.push_back(originPositionOfBody);
targetPositionVector.push_back(targetPositionOfBody);

double rms=ObservedPointFitter::findBestFit(system, state, indexVector, originPositionVector, targetPositionVector, 0.001);
system.realize(state,Stage::Position);
std::cout<<"Rms is: "<<rms<<std::endl;
}
catch (const std::exception& e)
{
std::cout<<"Exception: "<<std::string(e.what())+"\n";
}



return 0;
}


Thanks,

Jean-Philippe

[Michael Sherman]

Thanks, Jean-Phillipe. I was able to reproduce the problem here. It is definitely a bug in ObservedPointFitter where it fails if some of the bodies don't have any observed points on them. However the line that fails appears just to be an attempt to short-circuit an unnecessary computation; as a workaround you can just comment out lines 252&253 where it says "There are no stations ... affected by this."

An alternative that should work without having to rebuild ObservedPointFitter would be to add at least one target point to each body; you can apply low weights to the unwanted points.

We'll figure out a good fix for the next release.

[Jean-Philippe Jodoin]

Thanks a lot, I have compiled the last svn version of Simbody and it fixed it . Thanks again for the help.

Jean-Philippe