Add APIs to create sequentially an ArticulatedSystem

[diegoferigo]

The current version of RaiSim allows creating a model only by loading an existing URDF model.

In order to integrate the physics engine in other applications, it would be beneficial having APIs to programmatically add links, joints, visual (shapes and meshes) and collisions.

I assume that under the hood, the URDF parser at one point calls methods that add these entities into the simulation. What I mean, in other words, is that it would be nice having these methods publicly exposed.

Having this feature would allow users to use alternative descriptions than URDF, demanding their deserialization to user code.

[jhwangbo]

Is speed an issue? Technically you can modify your urdf after you load it in tinyxml. You can add joints and links as you want. When parsing urdf, RaiSim checks for errors in the description and computes necessary quantities to speed up simulation so initialization is inevitable. I believe creating a new system is in the order of tens of microseconds.

After URDF parsing, an articulated system description is in a form of raisim::Child https://github.com/leggedrobotics/raisimLib/blob/d9ba7b73de9ad1d1cb8cfbf24cbaaa7a9e0d9e81/include/raisim/object/ArticulatedSystem/JointAndBodies.hpp#L290 I can make an API that accepts an (user-created) instance of this class directly. Since this one is not part of the API, it is probably difficult to understand it now.

[diegoferigo]

Thanks @jhwangbo for the prompt reply. I had a look to the header you linked.

Is speed an issue? Technically you can modify your urdf after you load it in tinyxml. You can add joints and links as you want. When parsing urdf, RaiSim checks for errors in the description and computes necessary quantities to speed up simulation so initialization is inevitable. I believe creating a new system is in the order of tens of microseconds.

Editing the serialization is a solution, though it would be cumbersome to convert to an URDF model another kind of description (e.g. SDF, just to name something not supported). It would be more generic your second proposal, that is creating a Child object and constructing an ArticulatedSystem directly from it, possibly with a specific constructor.

Moreover, it would be a good and I think effortless addition being able to get the final Child object out from an ArticulatedSystem parsed from an URDF. The object is currently protected, and there are no public APIs to extract it.

These two features together would allow users to do the following to programmatically construct a model (correct me if I'm wrong):

1. Create an initial Child object with few joint, links, visuals, collisions
2. Create an ArticulatedSystem object from it
3. (Before starting the simulation) Get the Child from the ArticulatedSystem
4. Add more joints, links, visuals, collisions
5. Repeat from 2

Another bonus of this approach, if I got it right, would be that users could save Joint and Body object references from a Child object, and then just call e.g. myJoint.jointPosition(pos);. It would allow to bypass the array calls that are part of the ArticulatedSystem, which would require to know in advance the index of the specific joint (it could be calculated, but it is not easy to do in the case where the kinematic chain has both 1DoF and 3DoF joints, also because you should know in advance also the joint serialization order).

I can make an API that accepts an (user-created) instance of this class directly. Since this one is not part of the API, it is probably difficult to understand it now.

This would not be a problem, waiting the switch to public and documented APIs users can in the meantime start to experiment with it.

[jhwangbo]

It makes sense to have a constructor that takes a Child object. I'll make one. I still have to clean up that part of the code to expose it to users. It will be done by the end of this week.

Actually all data created in Child is restructured for better memory access and child_ object is cleared after the initialization. It is meant to be a description of the system only.

Getting the joint order when you randomly creating a robot can be tricky. I'll make a method that takes a movable joint name and returns its position. In that case, you can do

auto left_right_hip_idx = anymal_->getMovableJointIdx("left_right_hip");
auto angle = anymal_->getJointAngle(left_right_hip_idx);

Do you see any disadvantage of this approach in your use case?

[diegoferigo]

It makes sense to have a constructor that takes a Child object. I'll make one. I still have to clean up that part of the code to expose it to users. It will be done by the end of this week.

Amazing, thanks for addressing it so quickly (I don't have these hard time constraints in any case :)

Actually all data created in Child is restructured for better memory access and child_ object is cleared after the initialization.

I suspect that you do not create again the objects, right? You just change their order in the containers where they are stored. If this is the case, if users have a reference (or a pointer) of that object gathered before this re-arrangement, it should remain valid. Do you confirm?

Getting the joint order when you randomly creating a robot can be tricky. I'll make a method that takes a movable joint name and returns its position.

This is something else I felt missing. In order to fix the serialization I had to get all the non-fixed joints from the URDF before loading it in the ArticulatedSystem, and then use the optional joint list parameter to fix the order. The new method to get the joint index from the name would avoid doing all this clumsy procedure. What is the rationale behind the movable word? Is it due the reordering you mentioned? Probably getting the joint angle right from the joint name would be safer and would avoid users to store an index that possibly will become invalid. I don't think that passing through an unordered_map to get the real index from the string would add a notable overhead (access should be O(1) after all).

Would also spherical joints be supported? I think this method would be even more useful in models that contain multi-dofs joints, and in this case the returned value would be a Vec object I assume.

It is meant to be a description of the system only.

Do you mean that accessing Joint and Body objects from a Child object is not recommended? If yes, do you see any criticality of this approach? With clean APIs, I have the feeling that improving the usage of this Child class might provide a very powerful way to extract data from the model, particularly when dealing with just a single joint / body as opposed to the whole gc or gv vector (demanded to the ArticulatedSystem object).

---

Partially related to this discussion, I didn't find a way to extract the list of joint and body names. Maybe they could be gathered by the Child object once that it will be exposed. Though, do you think it is possible or useful to add APIs to the ArticulatedSystem also for this?

[jhwangbo]

I suspect that you do not create again the objects, right? ...

What I meant is that I stored the system description in completely different containers. For example, all joint positions are stored contiguously in memory. The notion of Child is gone.

What is the rationale behind the movable word?

yeah, 'movable' can be gone. Note that fixed joints disappear after initialization because they don't contribute to the dynamics. The only thing remains is the frame that stores its pose. It will give you "unknown joint" error if you ask for a fixed joint

I don't think that passing through an unordered_map to get the real index from the string would add a notable overhead (access should be O(1) after all).

True. I'll do that

Would also spherical joints be supported?

RaiSim supports a spherical joint since its release ;)

Do you mean that accessing Joint and Body objects from a Child object is not recommended? If yes, do you see any criticality of this approach? With clean APIs, I have the feeling that improving the usage of this Child class might provide a very powerful way to extract data from the model, particularly when dealing with just a single joint / body as opposed to the whole gc or gv vector (demanded to the ArticulatedSystem object).

As mentioned above, the data has to be stored in a particular way in memory for performance so I cannot keep a Child class object. But I can create an accessor class for something like this

auto anymal_ = world_->addArticulatedSystem("path");
auto leftFrontShank_ = anymal_->getBody("left_front_shank");
auto leftFrontShankPosition = leftFrontShank_->getPosition();
auto leftFrontThigh_ = leftFrontShank_->getParent();

Here the body object only stores an index and a pointer to the ArticulatedSystem. All dirty work for finding index will be done inside the class method. How does it sound?

Partially related to this discussion, I didn't find a way to extract the list of joint and body names. Maybe they could be gathered by the Child object once that it will be exposed. Though, do you think it is possible or useful to add APIs to the ArticulatedSystem also for this?

AS has "getBodyNames" method which only returns movable bodies. Rigidly fixed bodies are not stored here. Is there any reason to get fixed bodies? because URDF does not have a body frame. It only has a frame for joints. There is no meaningful information to extract from fixed bodies.

It is missing "getJointNames" method. I'll add it.

[diegoferigo]

What I meant is that I stored the system description in completely different containers. For example, all joint positions are stored contiguously in memory. The notion of Child is gone.

Ow now I see, thanks for the insight! Than, forget my previous comments, in light of this they are garbage. At this point all possible APIs should be directly added to the ArticulatedSystem object, I got it.

Here the body object only stores an index and a pointer to the ArticulatedSystem. All dirty work for finding index will be done inside the class method. How does it sound?

This is the natural alternative, it sounds really good! Would this object be const (i.e. it can be used only to extract data)? If a similar approach could be done also for joints, maybe through a non-const object users could also set references of change control modes of a single joint.

AS has "getBodyNames" method which only returns movable bodies. Rigidly fixed bodies are not stored here. Is there any reason to get fixed bodies? because URDF does not have a body frame. It only has a frame for joints. There is no meaningful information to extract from fixed bodies.

No sorry, I missed the method. I'm still familiarizing with the APIs.

It is missing "getJointNames" method. I'll add it.

Cool, thanks!

[jhwangbo]

If a similar approach could be done also for joints, maybe through a non-const object users could also set references of change control modes of a single joint.

Yes, some methods will be non-const. In my work, I also randomize collision bodies, link length and joint angles. This API is there but it is a bit difficult to use. The new body and joint API will make it easier. But you have to be careful in changing them because they might become an unrealistic model. For example, an internal collision can just lock the joint.

[diegoferigo]

Yes, some methods will be non-const. In my work, I also randomize collision bodies, link length and joint angles. This API is there but it is a bit difficult to use. The new body and joint API will make it easier. But you have to be careful in changing them because they might become an unrealistic model. For example, an internal collision can just lock the joint.

Great, thanks for today's discussion. Eager to see this coming.

[jhwangbo]

changes are pushed. let me know if you see anything missing!

[diegoferigo]

Thank you so much @jhwangbo, I'll have a look to the changes in the next few days and I let you know :)