Problems adding and removing object in the world

[ClaudioChiariello]

Hello, I am trying to train the ANYmal in raisim adding boxes in order to arrange steps. The porblem of course, is that the training time slow down drastically. Therefore I decided to insert object only after some episodes. To do this, I putted the following code in the 'curriculumUpdate' of Environment.hpp:

std::vector<raisim::Box*> boxes

   if(num_episode ==100){
      for (int i=0; i<num_steps; i++){
        boxes.push_back(world_->addBox(width_step -i, width_step-i, height_step, 10)); 
        boxes[i]->setName("step" + std::to_string(i)+"-th");
        boxes[i]->setPosition(15,0,0.5*i);
        //steps_indices[it] = *it->getIndexInWorld();  ///update the indices of the steps.
      }
    }   

The problem is that when I launch the runner.py, when it reach the episode 100, return me this error:

[2023:02:07:13:12:23 Reward.hpp:46] forwardVel is nan
[2023:02:07:13:12:23 Reward.hpp:46] forwardVel is nan
Segmentation fault (core dumped)

I don't know how to fix, the only thing it seems to works is to put that code into the constructor and then update the shape of the blocks in the curriculumupdate() function. There is something that I missed?

[jhwangbo]

do you check where ANYmal is? if the boxes are piercing ANYmal unrealistically, the simulation might be unstable

[ClaudioChiariello]

yes. I estimated the position with anymal_->getFramePosition(baseFrameIdx, anymal_pos_); and then added when I set the boxes position with: boxes[i]->setPosition(15,0,0.5*i); but the same error still occur

[ClaudioChiariello]

I was reading your last comment in the other issue. Should I manage the server in a particular way in the curriculumupdate() function? The problem maybe is that I am adding and removing object in simulation. But I am not sure how server->lockVisualizationServerMutex() and world->integrate work.

I've thought something like this:

  if(server_)
      server_->lockVisualizationServerMutex();

    if(num_episode ==1){
      raisim::SingleBodyObject* box = nullptr;
      for (int i=0; i<num_steps; i++){
        boxes.push_back(world_->addBox(width_step -i, width_step-i, height_step, 10)); 
        box = boxes.back();
        box->setName("step" + std::to_string(i)+"-th");
        box->setPosition(anymal_pos_[0] + 15,0,0.5*i);
      }
    }
    if(server_)
      server_->unlockVisualizationServerMutex();

[jhwangbo]

Your mutex code is correct. You can probably put the mutex code inside if (num_episode==1)...

What I meant is that you should not place ANYmal and other objects with an overlap. You should check the position of the ANYmal and place the boxes away from it.

[ClaudioChiariello]

Initially I was not convinced, but you're right. I think there were a problem with the function setPosition(), maybe because I called it with an object of raisim::Box* and not raisim::SingleBodyObject*

I still have some doubt about world.integrate() and server->lockVisualizationServerMutex()`` because often this part of code are inside a for loop. If I remember well, even in the code you put yesterday to answer to another issue, you usedworld.integrate()``` inside a for loop. Could you delight me please?

Anyways, I'm very glad for your help, I was desperate and now something works. Thanks a lot

[jhwangbo]

I didn't get your question. world.integrate should be between the mutex calls. This means that your main thread is locking the mutex so that the visualization thread cannot access variables of world. It can also be inside a for loop

[ClaudioChiariello]

Hi, I solved the problem to add boxes in the world. But currently, when the ANYmal is trained in an environment which contains object, the simulation goes extremely slow. Just few moment before to add the boxes, the quadruped walk and trot with, but after that object have been added, it moves very very slowly.

The training algorithm is the same provided by raisim. Most likely, you know that in Environment.hpp is defined the step function for the RL algorithm:

 float step(const Eigen::Ref<EigenVec>& action) {
    /// action scaling

    pTarget12_ = action.cast<double>();
    current_action_ = pTarget12_;  //I put the last action in the observation space

    pTarget12_ = pTarget12_.cwiseProduct(actionStd_);
    pTarget12_ += actionMean_;
    pTarget_.tail(nJoints_) = pTarget12_;

    anymal_->setPdTarget(pTarget_, vTarget_);

    for(int i=0; i< int(control_dt_ / simulation_dt_ + 1e-10); i++){
      if(server_) server_->lockVisualizationServerMutex();
      world_->integrate();
      if(server_) server_->unlockVisualizationServerMutex();
    }
   //code
  return reward.sum();
  }

I added the function which insert the boxes in another world:

void addBoxesIntoTheWorld(){
  if(server_)
      server_->lockVisualizationServerMutex();

    if(num_episode ==1){
      raisim::SingleBodyObject* box = nullptr;
      for (int i=0; i<num_steps; i++){
        boxes.push_back(world_->addBox(width_step -i, width_step-i, height_step, 10)); 
        box = boxes.back();
        box->setName("step" + std::to_string(i)+"-th");
        box->setPosition(anymal_pos_[0] + 15,0,0.5*i);
      }
    }
    if(server_)
      server_->unlockVisualizationServerMutex();
}

Things works quite well, the object spawn and I haven't penetration problem (in the tester.py, penetration occurs but I have some idea to cope with that). But the ANYmal become extremely slow. I don't know if these are problem related to my hardware, or because I miss something.

[jhwangbo]

Sorry for replying late. Your simulation will slow down if you add more than 20 boxes. Raisim is not faster than other physics engines in terms of collision checking. It is designed for simulating a small number of objects really fast.