[Question] Collision Detection for moving robot

[SirWalross]

How can you do collision detection for a moving robot based on a kinematic file and rotation of each joint?

If you just load the robot in as a scene:

rl::sg::solid::Scene scene;
scene.load("robot.xml");

how would you change the stance of the robot?

Thank you Very grateful for this project

[rickertm]

A good example for a mobile manipulator is the UMan (see kinematics and geometry models), that combines a Nomad XR4000 base with a Barrett WAM manipulator.

In order to update the geometry model using the kinematics model and a given configuration q, you first have to calculate the forward position kinematics, either using the rl::mdl API based on Spatial Vector Algebra or the (deprecated) rl::kin API based on the classical Denavit-Hartenberg notation:

rl::mdl::XmlFactory factory;
std::shared_ptr<rl::mdl::Model> model(factory.create("model.xml"));
rl::mdl::Kinematic* kinematic = dynamic_cast<rl::mdl::Kinematic*>(model.get());
kinematic->setPosition(q);
kinematic->forwardPosition();

You can then update the world frames of the individual bodies of your geometry model based on the calculated frames:

rl::sg::Model* model = scene.getModel(0);

for (std::size_t i = 0; i < model->getNumBodies(); ++i)
{
    model->getBody(i)->setFrame(kinematic->getBodyFrame(i)); // for current master branch
    // model->getBody(i)->setFrame(kinematic->getFrame(i)); // for 0.7 branch
}

[SirWalross]

First of all thank you for your answer.

I directly have another question. How do you set posture data (meaning flip or non-flip and above or below...) when using inverse kinematic? I haven't found a function in the kinematics class to change it.

Thanks in advance

[rickertm]

rl::mdl currently only includes the iterative IK solvers rl::mdl::JacobianInverseKinematics and rl::mdl::NloptInverseKinematics, where you can specify a start configuration. The latter also allows setting a lower and upper bound on the joint values, e.g., to limit the search to specific quadrants. If the solver fails, it will retry with various random configurations within the given number of iterations and maximum duration.

The rl::kin::Puma class in rl::kin includes an analytical IK solver for a standard 6-DOF industrial manipulator with a spherical wrist. Here, you can specify the arm, elbow, and wrist parameters (left/right, above/below, flip/nonflip). I am currently working on adding something similar to rl::mdl, including all possible IK solutions (up to eight in this case).

[SirWalross]

An analytical solver in rl::mdl would be great.

I have two more questions.

First how do you convert a rl::sg::Shape to a rl::sg::bullet::Shapeto use in the bullet engine? I tried converting it implicitly, but that didn't work and using dynamic_cast gave me a segmentation fault.

Secondly if i try to load a scene with a robot model i made i get the error ::rl::sg::solid::Shape::create(SoVRMLShape* shape) - geometry not supported. Here is one of the links of the robot: link6.txt. Do you know what causes the error? (i use rl-0.7.0)

Thanks in advance :)

[rickertm]

A shape has to match the underlying engine, i.e., for a Bullet scene/model/body you also have to create a Bullet shape, either via the rl::sg::bullet::Shape constructor or via the create(SoVRMLShape*) function of an existing Bullet body. For the latter, the resulting rl::sg::Shape is of type rl::sg::bullet::Shape and can be cast to this type. However, you cannot cast a PQP shape to a Bullet shape.

The supported geometry types depend on the engine. Most collision engines do not support the IndexedLineSet (line 33) or PointSet (line 2979) geometries included in your example. If you prefer to keep the lines for visualization, you can use separate collision geometry files that remove these nodes when using Bullet.