Rigid Body Labels

[Maenzgi]

Dear All

I have implemented almost everything, but I noticed in this forum that we are not supposed to "hard code" the structure of the rigid body tree. I reformulated the function using a loop, however, I don't understand how I get the "labels" of the individual rigid bodies. The change of reference frames should stop, as soon as it reaches the "main" rigid body of the robot. So I wanted to ask, where in the given code is this information accessible? I assume it can be accessed through the index of those rigid bodies, but I don't know where this information is stored.

Thank you in advance!

[eastskykang]

You may want to use functions in the hints

 // TODO: Ex.1 Forward Kinematics
// implement subfunction getCoordsInParentQIdxFrameAfterRotation() first.
//
// Hint: you may want to use the following functions
// - getQIdxForJoint()
// - getParentQIdxOf()
// - getCoordsInParentQIdxFrameAfterRotation() 

as well as the following functions

• getJointForQIdx()
• getQAxis()

and finally the following tutorial slides:

I think this would be enough hint for you. Hope this helps!

[Maenzgi]

Thank you! But sadly, the hints don't change anything for my code. I've implemented the coordinate shifts already last week and it works. The problem is not that I don't understand the concept of FK, but that I don't know where in the code I can access the information I need. My best guess is, that the getCoordsInParentQIdxFrameAfterRotation() function, which returns the same frame if the index is below 2, should fulfill the part of the stopping criteria, however, this doesn't work. Everything crashes once I reach rigid body with index 5. (If I hard code this and exclude number 5, only one leg moves, but it does so correctly, so the FK I coded are correct). My thougtht was, that you probably have every rigid body stored with the information whether it belongs to the base or not, and I wanted to know how to access this information. Since you didn't mention this in your answer, I'll go and check if there is anything I've been missing. It is probably possible to implement without having access to this information.

Thank you anyway

[eastskykang]

@Maenzgi I briefly mentioned this (stopping criteria of recursion) while I was presenting the third slide up there during the last tutorial. Indeed, at the base, getCoordsInParentQIdxFrameAfterRotation should reach to the "stopping criteria" as you mentioned.

Recall that the first 3 components of generalized coordinates are base position and the next 3 components are base orientation, which is YXZ euler angle.

If you see the code of the function getCoordsInParentQIdxFrameAfterRotation

P3D GeneralizedCoordinatesRobotRepresentation::
    getCoordsInParentQIdxFrameAfterRotation(int qIndex, const P3D &pLocal) {
    // if qIndex <= 2, this q is a component of position of the base. 
    if (qIndex <= 2) return pLocal;

So you see, the stopping criteria of this recursion is already there: qIndex <= 2.

Then what should we do for qIndex=3, 4, 5?

Basically, you should somehow implement the following:

One way you can do this is, you just keep proceeding your iteration to nextQIndex = getParentQIdxOf(currQIndex) when currQIndex =3, 4, 5. Here, nextQIndex is nothing but nextQIndex = currQIndex-1 (please see the code of the function getParentQIdxOf) As soon as nextQIndex becomes nextQIndex=2, the stopping criteria will be triggered.

But then, you should be careful, because for qIndex=3, 4, 5, since these components are of base orientation, there's no parent joint. In this case, our logic should be different. Thus you can write a code somewhat like this:

RBJoint *j = getJointForQIdx(qIndex);

if (j != nullptr) {
    // normal cases: qIndex corresponds to the DOF of a joint
    // blabla...
    return ...;
}
else {
    // qIndex corresponds to the DOF of the base
    // blabla...
    return ...;
}

Does this help?

[Maenzgi]

Yes, thank you very much!