Contact forces on the "edge" of the heightmap and my workaround

[cyoahs]

Hello!

I am simulating a case in which an object collide with the edge of the stair (modeled by a HeightMap). I find that the contact norm is always normal to the surface of the heightmap, instead of the surface of the object. This phenomenon is infeasible in the real physical world, as shown in the following figure (mu = 0.1).

The workaround used by me is to model the stairs by a series of Box instead. Then we can get the correct contact forces as shown in the following figure.

Is this workaround a good practice? Or is there any other ideas to solve this problem?

Here is a simple example for my test:

#include <vector>
#include <Eigen/Core>

#include "raisim/World.hpp"
#include "raisim/RaisimServer.hpp"

int main(int argc, char *argv[])
{
  bool useBox = true;
  bool useHeightMap = false;
  if (argc > 1)
  {
    useBox = std::string(argv[1]) == "box";
    useHeightMap = std::string(argv[1]) == "hm";
  }

  raisim::World world;
  auto ball = world.addSphere(0.2, 100.0);
  int xsamples = 100;
  int ysamples = 2;
  std::vector<double> heights;

  for (int i = 0; i < ysamples; ++i)
  {
    for (int j = 0; j < xsamples; ++j)
    {
      heights.push_back(j < xsamples / 2 ? 0.0 : 1.0);
    }
  }

  if (useBox)
  {
    auto box = world.addBox(1.0, 1.0, 1.0, 100.0);
    box->setPosition(0.5, 0.0, 0.5);
    box->setBodyType(raisim::BodyType::STATIC);
  }

  if (useHeightMap)
  {
    auto ground = world.addHeightMap(xsamples, ysamples, 1.0, 1.0, 0.0, 0.0, heights);
  }

  world.setTimeStep(0.002);
  /// launch raisim server for visualization. Can be visualized using raisimUnity
  raisim::RaisimServer server(&world);
  server.launchServer();

  ball->setPosition(-0.1, 0.0, 1.6);

  /// contact
  world.setMaterialPairProp("default", "default", 0.1, 0.3, 3.0);

  for (int i=0; i<10000; i++) {
    raisim::MSLEEP(2);
    // server.integrateWorldThreadSafe();
    server.lockVisualizationServerMutex();
    world.integrate();
    server.unlockVisualizationServerMutex();

    auto contacts = ball->getContacts();
    if (!contacts.empty())
    {
      raisim::MSLEEP(1000);
      for (auto contact: contacts)
      {
        double sign = contact.isObjectA() ? 1.0 : -1.0;
        auto impulse = contact.getContactFrame().e().transpose() * contact.getImpulse().e() * sign;
        auto normal = contact.getNormal().e() * sign;
        RSINFO_IF(impulse.lpNorm<2>() > 0.1, std::endl
              << "Contact impulse: " << impulse.transpose() << std::endl
              << "Contact normal: " << normal.transpose() << std::endl);
      }
    }
  }

  server.killServer();
  std::cout<<"end of simulation"<<std::endl;
}

[Wang-Zhicheng]

Happy to know you.
I am also confused by this issue. I guess the latest version has fixed the problem.

How is the simulation speed of your workaround? RaiSim is using a "per-contact iteration solver" so I guess the contact point between boxes and the ground will be counted in when solving, and the time used will be longer than just using heightmaps.

my code:

#include <string>
#include <random>
#include "raisim/RaisimServer.hpp"
#include "raisim/World.hpp"

int main(int argc, char* argv[]) {
  auto binaryPath = raisim::Path::setFromArgv(argv[0]);
  raisim::World::setActivationKey(binaryPath.getDirectory() + "/../../rsc/activation.raisim");

  // create raisim world
  raisim::World world;
  world.setTimeStep(0.001);

  /// launch raisim server
  raisim::RaisimServer server(&world);
  server.launchServer();

  int STARTUP_PAD = 2;
  int COOL_DOWN_PAD = 2;
  int MAX_PIT_WIDTH = 10;
  double PIT_PROB = 0.3;
  double TERRAIN_STEP = 0.05;
  double xSize = 5.0;
  double ySize = 0.8;
  std::vector<int> groundCond;
  std::vector<double> hgt(4, 0.);
  groundCond.push_back(-10);
  srand(time(nullptr));
  for(int x=STARTUP_PAD;x<xSize/TERRAIN_STEP;x++){
    double samp = 1.0*rand()/RAND_MAX;
    if(samp<PIT_PROB){
      groundCond.push_back(x);
      x += ((int)rand())%MAX_PIT_WIDTH + 1;
      groundCond.push_back(x);
      x += COOL_DOWN_PAD;
    }
  }
  if(groundCond.size()%2==1){
    if(groundCond.back()*TERRAIN_STEP<xSize) groundCond.push_back((int)(xSize/TERRAIN_STEP));
    else groundCond.pop_back();
  }

  for (int i=0; i<groundCond.size()/2; i++) {
    std::cout << groundCond[2*i] << "..." << groundCond[2*i+1] <<std::endl;
    auto hm = world.addHeightMap(2, 2,
                 (groundCond[2*i+1]-groundCond[2*i])*TERRAIN_STEP, ySize,
                 (groundCond[2*i+1]+groundCond[2*i])*TERRAIN_STEP/2.0, 0., hgt);
    hm->setName("heightFrag"+std::__cxx11::to_string(i));
  }

  double cubeSize = 0.033;
  double cubeSamp = 0.035;
  Eigen::VectorXd cfg(7);
  for (int i=0; i<xSize/cubeSamp; i++) {
    for (int j=-1; j<2; j++) {
      auto bx = world.addBox(cubeSize, cubeSize, cubeSize, 10.0);
      bx->setPosition(i*cubeSamp, j*cubeSamp, 0.02);
    }
  }

  for (int i=0; i<200000000; i++) {
      std::this_thread::sleep_for(std::chrono::microseconds(1000));
      server.integrateWorldThreadSafe();
      if(i%80==0 && i<1200){
          std::string wait;
          std::cin >> wait;
      }
  }

  server.killServer();
}

[cyoahs]

I'm glad this discussion can help you with this issue.

I guess the latest version has fixed the problem.

However, I have not tested this problem on the newest version. Maybe you can have a try and post the result here.

RaiSim is using a "per-contact iteration solver" so I guess the contact point between boxes and the ground will be counted in when solving, and the time used will be longer than just using heightmaps.

I guess so. We use the collision group and mask feature to ignore the collisions among boxes, and thus it has limited effect to the speed. Please refer to this page.