Open Kenn3o3 opened 1 month ago
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
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_;
}
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.
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
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.