Making Custom Changes to Robot Model using RL

[danielj195]

Hi there,

We are wanting to add one more link and frame to the robot model that we're importing from a URDF. Could you provide some sample code on constructing robot models (kinematic and dynamic) using the rl::mdl namespace?

Currently our code looks something like this:

rl::mdl::Frame *new_frame = new rl::mdl::Frame();
new_frame->setName("new_frame");
model_->add(new_frame);

new_frame_fixed_link_ = std::make_shared<rl::mdl::Fixed>();
model_->add(new_frame_fixed_link_.get(), eef_frame, new_frame);
new_frame_fixed_link_->setName("eef_to_new_link");
model_->update();

When we try to run kinematic functions using this code, we're getting a segmentation fault. However, the code works just fine when we use the URDF that we import. If you could post some sample code showing how to construct a model and/or add links to models, we would greatly appreciate it!

[rickertm]

The segmentation fault is caused by the shared pointer for the new_frame_fixed_link_ object. The rl::mdl::Model::add functions reset the internal shared pointer in the tree structure with the provided pointer, therefore two shared pointers are trying to release the same pointer in the end.

The following code should work:

rl::mdl::Frame* new_frame = new rl::mdl::Frame();
new_frame->setName("new_frame");
model_->add(new_frame);

rl::mdl::Fixed* new_frame_fixed_link_ = new rl::mdl::Fixed();
model_->add(new_frame_fixed_link_, eef_frame, new_frame);
new_frame_fixed_link_->setName("eef_to_new_link");
model_->update();

I'll update the API to also allow handing over a shared pointer directly.