[QUESTION] Attach object in end effector

marcoojer commented 3 years ago

Hi, i want to plan a robot which is holding and object on the end effector. Thus, I only want to take into account the collision of the object but still plan for the end effector position of the robot. Thus, I don't know how to add the object just in the scene and constantly update its pose according to the tool, because i'm not adding this object to the kinematic model in order to preserve the robot tool as operating frame. Can anyone provide a sample of the code about how to do it?

rickertm commented 3 years ago

The bodies/links of a robot's geometric model in a collision scene are synchronized with its kinematic model by setting the frames of its bodies in the scene, typically after calculating the forward position kinematics (see here for how this is done in rl::plan::Model).

In your case, you can simply create a collision scene model with only one body (your object) and update its frame with the robot's tool frame:

mdlModel->setPosition(q);
mdlModel->forwardPosition();
rl::math::Transform frame = mdlModel->getOperationalPosition(0);
sgModel->getBody(0)->setFrame(frame);

Other options include changing all body elements in the kinematic model to frame elements, except for one body element for your object attached to the end effector. Or you can create empty geometric objects in your scene for all bodies except the last one, similar to the first three links of the UMan mobile manipulator geometry model in rl-examples.

marcoojer commented 3 years ago

Thank you very much!!!, I understand. Then If I want to make a plan with the object in the end effector, I should subclass rl::plan::Model in order to modify the updateFrames function and also I should disable the collision check between the end effector link body and the object body. Is Collision disabling between models of the scene implemented or it has to be implemented in the subclass?

rickertm commented 3 years ago

If I understand you correctly, you would like to plan a collision-free path with this setup, not only check for collisions. You would like to ignore collisions of the robot model itself and only check for collisions with the object the robot is holding. In this case, subclassing rl::plan::Model should not be necessary when using the latter two options mentioned above.

In the first option, you create a collision scene description with a model for your robot+object that only has one body, i.e., the object the robot is holding. Obstacles in your environment can be placed as bodies in other models in the same file. In your kinematic model, the links of the robots are not modeled as <body> elements, only as <frame> elements. The object itself is modeled as a <body> element attached to the frame of the end effector via a transform edge. rl::plan::Model::updateFrames then only has one body to synchronize.

In the second option, you keep the <body> elements of your robot's links in the kinematic model and add another <body> element for your object as mentioned above (or you can combine this with the body for the last link in your geometric model). In your collision scene XML description, you now have the bodies of the robot plus the body of your object. In the corresponding VRML file, you use empty nodes for the links of the robot similar to the UMan example file to skip the collision checks. rl::plan::Model::updateFrames will then synchronize all bodies, but uses empty objects for the links.

Please note, that collisions of the robot with the environment are not detected in this case and make sure the path is safe before running this on a real system.

roboticslibrary / rl

[QUESTION] Attach object in end effector #43