How to import real world 3d map into Raisim

[Kenn3o3]

Hello,
We have successfully downloaded 3D data like gibson/matterport (from: https://github.com/StanfordVL/GibsonEnv/blob/master/gibson/data/README.md). The dataset contain mesh data(*.glb, *.navmesh) of real world houses and can be run on simulators like habitat-sim.
May I know if there is a way to import real world 3D map data like that into the RaiSim simulator for training a quadrupedal robot?
Thank you very much.

[jhwangbo]

We use polycam to record real world map and simulate it in raisim. Raisim uses assimp to visualize mesh, but it only accepts obj for collision. If you can convert it properly then it should work

[JJPlummer]

I have the same problem here. I tried to load a visual mesh into the world just like the maps examples for raisimunreal to train the robot but it can only be imported as an object and the robot was able to move the mesh around. I want the mesh to be stationary and allow the robot to move around within the mesh. What do you suggest I do?

Thank you in advance!

My simulation:

My Environment.hpp:

#include <random>
#include <iostream>
#include <stdlib.h>
#include <set>
#include "../../RaisimGymEnv.hpp"

namespace raisim {

class ENVIRONMENT : public RaisimGymEnv {

 public:

  explicit ENVIRONMENT(const std::string& resourceDir, const Yaml::Node& cfg, bool visualizable) :
      RaisimGymEnv(resourceDir, cfg), visualizable_(visualizable), normDist_(0, 1) {

    // Create world
    world_ = std::make_unique<raisim::World>();

    // Add objects
    anymal_ = world_->addArticulatedSystem(resourceDir_+"/anymal_c/urdf/anymal.urdf");
    anymal_->setName("anymal");
    anymal_->setControlMode(raisim::ControlMode::PD_PLUS_FEEDFORWARD_TORQUE);

    world_->addGround();
    Mesh* house = world_->addMesh("/home/user1/Desktop/quadrupedal-2024/code/raisimLib/raisimPy/examples/Allensville.obj", 2000.0, 3);
    house->setPosition(0,0,0);
    // Add green cylinder
    cylinder_ = world_->addCylinder(0.1, 1.0, 1.0);
    cylinder_->setAppearance("green"); // Green color
    cylinder_->setPosition(10.0, -5.0, 0.5); // Adjust position as needed
    cylinder_->setBodyType(BodyType::STATIC);

    // Get robot data
    gcDim_ = anymal_->getGeneralizedCoordinateDim();
    gvDim_ = anymal_->getDOF();
    nJoints_ = gvDim_ - 6;

    // Initialize containers
    gc_.setZero(gcDim_); gc_init_.setZero(gcDim_);
    gv_.setZero(gvDim_); gv_init_.setZero(gvDim_);
    pTarget_.setZero(gcDim_); vTarget_.setZero(gvDim_); pTarget12_.setZero(nJoints_);

    // Nominal configuration of anymal
    gc_init_ << 0, 0, 0.54, 1, 0, 0, 0, 0.03, 0.4, -0.8, -0.03, 0.4, -0.8, 0.03, -0.4, 0.8, -0.03, -0.4, 0.8;


    // Set PD gains
    Eigen::VectorXd jointPgain(gvDim_), jointDgain(gvDim_);
    jointPgain.setZero(); jointPgain.tail(nJoints_).setConstant(50.0);
    jointDgain.setZero(); jointDgain.tail(nJoints_).setConstant(0.2);
    anymal_->setPdGains(jointPgain, jointDgain);
    anymal_->setGeneralizedForce(Eigen::VectorXd::Zero(gvDim_));

    // MUST BE DONE FOR ALL ENVIRONMENTS
    obDim_ = 34 + 3; // Additional 3 dimensions for the relative position of the cylinder
    actionDim_ = nJoints_; actionMean_.setZero(actionDim_); actionStd_.setZero(actionDim_);
    obDouble_.setZero(obDim_);

    // Action scaling
    actionMean_ = gc_init_.tail(nJoints_);
    double action_std;
    READ_YAML(double, action_std, cfg_["action_std"])
    actionStd_.setConstant(action_std);

    // Reward coefficients
    rewards_.initializeFromConfigurationFile (cfg["reward"]);

    // Indices of links that should not make contact with ground
    footIndices_.insert(anymal_->getBodyIdx("LF_SHANK"));
    footIndices_.insert(anymal_->getBodyIdx("RF_SHANK"));
    footIndices_.insert(anymal_->getBodyIdx("LH_SHANK"));
    footIndices_.insert(anymal_->getBodyIdx("RH_SHANK"));

    // Visualize if it is the first environment
    if (visualizable_) {
      server_ = std::make_unique<raisim::RaisimServer>(world_.get());
      server_->launchServer();
      server_->focusOn(anymal_);
    }


  }

  void init() final { }

  void reset() final {
    anymal_->setState(gc_init_, gv_init_);
    updateObservation();
  }

  float step(const Eigen::Ref<EigenVec>& action) final {
    Eigen::Vector3d anymalPosition = Eigen::Vector3d(anymal_->getBasePosition()[0],
                                                     anymal_->getBasePosition()[1],
                                                     anymal_->getBasePosition()[2]);
    Eigen::Vector3d cylinderPosition = cylinder_->getPosition();
    Eigen::Vector3d toCylinder = (cylinderPosition - anymalPosition).normalized();

    // Get the forward direction of the robot from the rotation matrix
    raisim::Vec<4> quat;
    anymal_->getState(gc_, gv_);
    raisim::Mat<3,3> rot;  // Declare the rotation matrix here
    raisim::quatToRotMat({gc_[3], gc_[4], gc_[5], gc_[6]}, rot);
    Eigen::Vector3d forwardDirection = rot.e().col(0); // Assuming the forward direction is the first column

    // Calculate the alignment using the dot product
    double alignment = forwardDirection.dot(toCylinder);
    
    double distance = toCylinder.norm();
    bool done = distance < 0.1;
    if (done) {
        pTarget_.tail(nJoints_).setZero();
        vTarget_.tail(nJoints_).setZero();
        rewards_.record("reached_goal", 1000.0);
    } else {
        pTarget12_ = action.cast<double>();
        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();
    }

    updateObservation();

    
    rewards_.record("distance", -distance);
    rewards_.record("torque", anymal_->getGeneralizedForce().squaredNorm());
    rewards_.record("forwardVel", std::min(4.0, bodyLinearVel_[0]));
    rewards_.record("alignment", alignment); // Add this line to include alignment reward

    return rewards_.sum();
  }

 
  void updateObservation() {
    anymal_->getState(gc_, gv_);
    raisim::Vec<4> quat;
    raisim::Mat<3,3> rot;
    quat[0] = gc_[3]; quat[1] = gc_[4]; quat[2] = gc_[5]; quat[3] = gc_[6];
    raisim::quatToRotMat(quat, rot);
    bodyLinearVel_ = rot.e().transpose() * gv_.segment(0, 3);
    bodyAngularVel_ = rot.e().transpose() * gv_.segment(3, 3);
    raisim::Vec<3> cylinderPosition;
    
    Eigen::Vector3d anymalPosition = Eigen::Vector3d(anymal_->getBasePosition()[0],
                                                     anymal_->getBasePosition()[1],
                                                     anymal_->getBasePosition()[2]);
    double relPos = (cylinder_->getPosition() - anymalPosition).norm();   

    obDouble_ << gc_[2], // body height
        rot.e().row(2).transpose(), // body orientation
        gc_.tail(12), // joint angles
        bodyLinearVel_, bodyAngularVel_, // body linear&angular velocity
        gv_.tail(12), // joint velocity
        relPos; // relative position to the cylinder
  }

  void saveVectorsToFile(const std::vector<raisim::Vec<3>>& vectors, const std::string& filename) {
      std::ofstream outFile(filename);
      for (const auto& vec : vectors) {
          outFile << vec[0] << "," << vec[1] << "," << vec[2] << std::endl;
      }
      outFile.close();
  }

  void observe(Eigen::Ref<EigenVec> ob) final {
    ob = obDouble_.cast<float>();
  }

  bool isTerminalState(float& terminalReward) final {
    terminalReward = float(terminalRewardCoeff_);

    // if the contact body is not feet
    for(auto& contact: anymal_->getContacts())
      if(footIndices_.find(contact.getlocalBodyIndex()) == footIndices_.end())
        return true;

    terminalReward = 0.f;
    return false;
  }

  void curriculumUpdate() { };

 private:
  int gcDim_, gvDim_, nJoints_;
  bool visualizable_ = false;
  raisim::ArticulatedSystem* anymal_;
  raisim::Cylinder* cylinder_; // Add cylinder object
  Eigen::VectorXd gc_init_, gv_init_, gc_, gv_, pTarget_, pTarget12_, vTarget_;
  double terminalRewardCoeff_ = -10.;
  Eigen::VectorXd actionMean_, actionStd_, obDouble_;
  Eigen::Vector3d bodyLinearVel_, bodyAngularVel_;
  std::set<size_t> footIndices_;

  std::normal_distribution<double> normDist_;
  thread_local static std::mt19937 gen_;

};
thread_local std::mt19937 raisim::ENVIRONMENT::gen_;

}

[JJPlummer]

It takes a very long time for each iteration as well after the mesh has been imported. Without it takes around 0.25 seconds per iteration.

[jhwangbo]

This speed is expected. Probably there are million triangles and million collision checks...

if you share the mesh file, i can check what is going on