[Ask] How to use mjSpec functions (How to attach and detach a xml model in simulation ?) #2228

kshfire · 2024-11-14T10:22:56Z

kshfire
Nov 14, 2024

Intro

Hi!

I want to attach or detach a xml (ex. Gripper tool) in run-time.
I've tried to apply mjSpec functions, but I can't understand about mjSpec functions and failed.

I tried to refer "user_api_test.cc file: ex. TEST_F(MujocoTest, RecompileAttach) {

However, although MuJoCo is very nice library, but sometime I've suffered a lot because I don't know how to use it and the Document is not kind to me.

Anyway, I'm really appreciated it if someone teach me how to attach or detach a gripper xml model in run time.

Thanks.

My setup

MuJoCo, C, Windows

Using simulate.cc source code.

My question

I want to know how to attach or detach a xml model in run time.

Ex. I want to attach or detach a gripper xml model on the Robot Flange. (During run time)

In addition, Maybe due to thread synchronize problem (using simulate source code), sometimes segfault, somtimes (for test) sphere is attached. However, it is not rigidly attached. When I run the simulation, the attached sphere is flied away. (?? why it is not attached rigidly ??) How to attach it rigidly ?? mjSpec functions are very difficult to me :) please someone save me.

When I tested below code : the result is segmentation fault as follows.

PhysicsLoop : specGripper: 0x7f3230e77ff0
xmlSpec: 0x7f3230005ad0
bodyFlange: 0x7f323006e8c8
attachmentFrame: 0x7f3230e868b0
Segmentation fault

Minimal model and/or code that explain my question

If you encountered the issue in a complex model, please simplify it as much as possible (while still reproducing the issue).

Model:

minimal XML

TEST_F(MujocoTest, RecompileAttach) {
  std::array<char, 1000> er;
  std::string field = "";

  static constexpr char xml[] = R"(
  <mujoco>
    <worldbody>
      <body name="body">
        <joint type="slide" axis="0 0 1"/>
        <geom size=".2"/>
      </body>
    </worldbody>
  </mujoco>)";

  mjSpec* parent = mj_makeSpec();
  EXPECT_THAT(parent, NotNull());

  mjSpec* child = mj_parseXMLString(xml, 0, er.data(), er.size());
  EXPECT_THAT(child, NotNull());

  mjsFrame* frame1 = mjs_addFrame(mjs_findBody(parent, "world"), 0);
  mjs_attachBody(frame1, mjs_findBody(child, "body"), "child-", "-1");

  mjModel* model = mj_compile(parent, 0);
  EXPECT_THAT(model, NotNull());
  EXPECT_THAT(model->nq, 1);
  mjData* data = mj_makeData(model);
  EXPECT_THAT(data, NotNull());

  for (int i = 0; i < 100; i++) {
    mj_step(model, data);
  }

  mjsFrame* frame2 = mjs_addFrame(mjs_findBody(parent, "world"), 0);
  mjs_attachBody(frame2, mjs_findBody(child, "body"), "child-", "-2");

  EXPECT_EQ(mj_recompile(parent, 0, model, data), 0);

Code:

// run until asked to exit
while (!sim.exitrequest.load()) {

.....

    std::array<char, 1000> err;
    // robotiq_2f85 : 2f85.xml gripper
    specGripper = mj_parseXML(gripperXMLPath.c_str(), nullptr, err.data(), err.size());
    if (specGripper == nullptr) {
      std::cout << "Gripper parse error: " << err.data() << '\n';
    }

   // my robot's spec : I want to attach the gripper on the flange part.
   auto bodyFlange = mjs_findBody(xmlSpec, "body_link6");            
   std::cout << "bodyFlange: " << bodyFlange << '\n';
   auto attachmentFrame = mjs_addFrame(bodyFlange, 0);
   std::cout << "attachmentFrame: " << attachmentFrame << '\n';

  auto world = mjs_findBody(specGripper, "world");
  mjs_attachBody(attachmentFrame, world, "attached-", "-1");

  mj_recompile(xmlSpec, nullptr, m, d);

//======================================================
   if (sim.droploadrequest.load()) {

Confirmations

I searched the latest documentation thoroughly before posting.
I searched previous Issues and Discussions, I am certain this has not been raised before.

quagla · 2024-11-14T10:40:20Z

quagla
Nov 14, 2024
Collaborator

Hi the functions that you need are detach_body and recompile.

The function detach_body is quite straightforward, you need to pass the body you want to detach. You can look for this body into the spec using spec.find_body().

See an example of how to use recompile here https://github.com/google-deepmind/mujoco/blob/main/python/mujoco/specs_test.py#L509 note that recompile creates new model and data.

Remember to not load the model directly as in your example above, instead do:

spec = mujoco.MjSpec.from_file(xml)
model = spec.compile()
data = mujoco.MjData(model)

13 replies

kshfire Nov 20, 2024
Author

Thank you :). I will check the colab link.

kshfire Nov 20, 2024
Author

Thanks quagla.
I just realized why a sphere was flied away when simulation is started.

There was a joint in the Sphere xml ;;;
It is totally my mistake...

If I can attach a gripper to a robot flange using mjSpec, I'll share happy result.

I may realized why the simulate terminated when a gripper was tried to attach using mjSpec
In the simulate code, I also implemented dynamics-based control logics.
When model is recompiled by mjSpec, I found that several attributes of the model such as nq, Number of joint, etc are also changed.
- It was conflicted with my dynamics-based control logics and so simulate process was terminated...;;;
Since this is my first time to use the mjSpec function, I don't care about it : model attributes are also changed... ;;; so I need to take care about this change when implement some logics using MuJoCo APIs. But I didn't ^^;;

kshfire Nov 20, 2024
Author

I'm so sorry but I need your help (@quagla)
I did some tests using mujoco_menagerie : UR5e robot and robotiq_2f85 gripper

Simliar to the attached colab, I've tested mjSpec function at the jupyter notebook.

I attached robotiq_2f85 gripper (base_mount body) to UR5e's wrist_3_link body.
It was OK (gripper orientation is not proper but it is not a problem (can fix it later))

However, when I did similar test at C++ (modified simulate.cc a little bit), it could not work.

@quagla If you don't mind, could you check/test it in C++ case ?
I not sure that mjSpec of Python is Ok but mjSpec of C++ has a problem ??

Belows are what I modified to test it:

After build I run like this
./simulate "UR5e xml path"

main.cc :
`
// global variable:
mjSpec *xmlSpec = nullptr;
mjSpec *specGripper = nullptr;
// for gripper xml
std::string gripperXMLPath = "gripper xml path"

// For Spec : Use mj_parseXML instead of mj_loadXML
mjModel *LoadModelByMjs(const char *file, mj::Simulate &sim)
{
// this copy is needed so that the mju::strlen call below compiles
char filename[mj::Simulate::kMaxFilenameLength];
mju::strcpy_arr(filename, file);

// make sure filename is not empty
if (!filename[0]) {
  return nullptr;
}

// load and compile
char loadError[kErrorLength] = "";
mjModel *mnew = 0;
std::array<char, 1000> er;
auto load_start = mj::Simulate::Clock::now();
if (mju::strlen_arr(filename) > 4 &&
    !std::strncmp(filename + mju::strlen_arr(filename) - 4, ".mjb",
                  mju::sizeof_arr(filename) - mju::strlen_arr(filename) +
                      4)) {
  xmlSpec = mj_parseXML(filename, nullptr, er.data(), er.size());
  if (!xmlSpec) {
    mju::strcpy_arr(loadError, "could not parse spec from XML file");
  }
} else {
  xmlSpec = mj_parseXML(filename, nullptr, loadError, kErrorLength);

  // remove trailing newline character from loadError
  if (loadError[0]) {
    int error_length = mju::strlen_arr(loadError);
    if (loadError[error_length - 1] == '\n') {
      loadError[error_length - 1] = '\0';
    }
  }
}

// compile models
mnew = mj_compile(xmlSpec, nullptr);

auto load_interval = mj::Simulate::Clock::now() - load_start;
double load_seconds = Seconds(load_interval).count();

// if no error and load took more than 1/4 seconds, report load time
if (!loadError[0] && load_seconds > 0.25) {
  mju::sprintf_arr(loadError, "Model loaded in %.2g seconds", load_seconds);
}

mju::strcpy_arr(sim.load_error, loadError);

if (!mnew) {
  std::printf("%s\n", loadError);
  return nullptr;
}

// compiler warning: print and pause
if (loadError[0]) {
  // mj_forward() below will print the warning message
  std::printf("Model compiled, but simulation warning (paused):\n  %s\n",
              loadError);
  sim.run = 0;
}

if (!mnew) {
  std::printf("%s\n", loadError);
  mju::strcpy_arr(sim.load_error, loadError);
  return nullptr;
}

// compiler warning: print and pause
if (loadError[0]) {
  // mj_forward() below will print the warning message
  std::printf("Model compiled, but simulation warning (paused):\n  %s\n",
              loadError);
  sim.run = 0;
}

// if no error and load took more than 1/4 seconds, report load time
else if (load_seconds > 0.25) {
  mju::sprintf_arr(loadError, "Model loaded in %.2g seconds", load_seconds);
}

mju::strcpy_arr(sim.load_error, loadError);

return mnew;

}

// simulate in background thread (while rendering in main thread)
void PhysicsLoop(mj::Simulate &sim) {
....

// For Test
uint64_t count = 0;

// run until asked to exit
while (!sim.exitrequest.load()) {
++count;

// mjSpec Test
if (count <= 2000 && count % 2000 == 0) {
  // if (false) {
  printf("count MOD 2000 == 0\n");
  printf("Load gripper xml\n");

  std::cout << "xmlSpec: " << xmlSpec << '\n';

  std::array<char, 1000> err;
  specGripper =
      mj_parseXML(gripperXMLPath.c_str(), nullptr, err.data(), err.size());
  if (specGripper == nullptr) {
    std::cout << "Gripper parse error: " << err.data() << '\n';
  }
  std::cout << __FUNCTION__ << " : specGripper: " << specGripper << '\n';

  auto bodyFlange = mjs_findBody(xmlSpec, "wrist_3_link");
  std::cout << "bodyFlange: " << bodyFlange << '\n';
  auto testFrame = mjsFrame();
  testFrame.pos[1] = 0.1;
  auto attachmentFrame = mjs_addFrame(bodyFlange, &testFrame);
  std::cout << "attachmentFrame: " << attachmentFrame << '\n';

  auto baseGripper = mjs_findBody(specGripper, "base_mount");
  if (mjs_attachBody(attachmentFrame, baseGripper, "attached-", "-1") == nullptr) {
    std::cout << "mjs_attachBody(attachmentFrame, world, , ) is failed"
              << '\n';
    break;
  }

  {
    std::cout << "Before m : " << m << ". d: " << d << '\n';

    mj_recompile(xmlSpec, nullptr, m, d);

    std::cout << "After m : " << m << ". d: " << d << '\n';

    std::cout << "xmlSpec contents:\n";

    int size = mj_saveXMLString(xmlSpec, nullptr, 0, nullptr, 0);
    std::unique_ptr<char[]> buf(new char[size + 1]);
    std::array<char, 1024> err;
    buf[0] = '\0';
    err[0] = '\0';
    mj_saveXMLString(xmlSpec, buf.get(), size + 1, err.data(), err.size());
    // if (err[0] != '\0') { throw FatalError(std::string(err.data())); }
    std::cout << std::string(buf.get());

  }

  printf("Xml recompile complete\n");
}

if (sim.droploadrequest.load()) {

......

`

quagla Nov 20, 2024
Collaborator

Hi, the Python is based on the C, so it's extremely unlikely that there is an issue with C and not with Python. But if you give me a minimal example, I can have a look. See an excellent example of how to file an issue here #2061

kshfire Nov 21, 2024
Author

Ok. I will upload simple test code in this weekend. (@quagla)

A little bit frustrated ^^;;. I also found different responses between Jupyter notebook and pure Python cases.

Jupyter notebook is Ok. Your google colab codes are well worked. (Inevitable result)
I've implemented simple test codes by pure python script : It is not worked. Error is occured at recomple code.

new_model, new_data = specRobot.recompile(model, data)
ValueError: Error: engine: Python exception raised

I wll re-check github's issues related to mjSpec recompile.

Thank you anyway.

kshfire · 2024-11-24T16:01:15Z

kshfire
Nov 24, 2024
Author

@quagla
I've attached some materials which I'm suffered the mjSpec and its recompile function problem.

I just uploaded python test code (I'll believe that python version is ok then c++ version will be ok, too.)

Test conditions

Windows OS (Ubuntu has same problem) (My actual target is Ubuntu (currently 24.04)
Python (My actual target is C++ : custumizing the simulate sample codes)
UR5e robot : Using MuJoCo Menagerie (https://github.com/google-deepmind/mujoco_menagerie)
Robotiq 2f85 : Using MuJoCo Menagerie

My test code is attached below
arm_work.zip

Problem:
Interestingly this simple program is executed (??). But, the test gripper is not well visualized as shown below

Please check my attached code and I'm thank you if you will provide me a solution for this problem.

0 replies

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[Ask] How to use mjSpec functions (How to attach and detach a xml model in simulation ?) #2228

{{title}}

{{editor}}'s edit

{{editor}}'s edit

Replies: 2 comments 13 replies

{{title}}

{{editor}}'s edit

{{editor}}'s edit

{{title}}

{{title}}

{{title}}

{{editor}}'s edit

{{editor}}'s edit

{{title}}

{{title}}

{{editor}}'s edit

{{editor}}'s edit

{{title}}

{{editor}}'s edit

{{editor}}'s edit

Select a reply

[Ask] How to use mjSpec functions (How to attach and detach a xml model in simulation ?) #2228

kshfire Nov 14, 2024

Intro

My setup

My question

Minimal model and/or code that explain my question

Confirmations

Replies: 2 comments · 13 replies

quagla Nov 14, 2024 Collaborator

kshfire Nov 20, 2024 Author

kshfire Nov 20, 2024 Author

kshfire Nov 20, 2024 Author

quagla Nov 20, 2024 Collaborator

kshfire Nov 21, 2024 Author

kshfire Nov 24, 2024 Author

kshfire
Nov 14, 2024

Replies: 2 comments 13 replies

quagla
Nov 14, 2024
Collaborator

kshfire Nov 20, 2024
Author

kshfire Nov 20, 2024
Author

kshfire Nov 20, 2024
Author

quagla Nov 20, 2024
Collaborator

kshfire Nov 21, 2024
Author

kshfire
Nov 24, 2024
Author