Skip to content
This repository has been archived by the owner on Dec 30, 2020. It is now read-only.

Commit

Permalink
WIP
Browse files Browse the repository at this point in the history
jelavice committed Sep 26, 2020

Unverified

This commit is not signed, but one or more authors requires that any commit attributed to them is signed.
1 parent b264efa commit fc4bde1
Showing 26 changed files with 1,330 additions and 5,648 deletions.
143 changes: 64 additions & 79 deletions raisim_gym/env/env/mabi/Environment.hpp
Original file line number Diff line number Diff line change
@@ -39,89 +39,78 @@
#include <set>
#include <raisim/OgreVis.hpp>
#include "visualizer/raisimKeyboardCallback.hpp"
#include "../../../../../collision_free_gym/raisim_gym/env/RaisimGymEnv.hpp"
#include "../../../../../collision_free_gym/raisim_gym/env/visualizer/guiState.hpp"
#include "../../../../../collision_free_gym/raisim_gym/env/visualizer/helper.hpp"
#include "../../../../../collision_free_gym/raisim_gym/env/visualizer/raisimBasicImguiPanel.hpp"
#include "../../../../../collision_free_gym/raisim_gym/env/visualizer/raisimKeyboardCallback.hpp"
#include "../mabi/visSetupCallback.hpp"
#include "RaisimGymEnv.hpp"
#include "visualizer/guiState.hpp"
#include "visualizer/helper.hpp"
#include "visualizer/raisimBasicImguiPanel.hpp"
#include "visualizer/raisimKeyboardCallback.hpp"
#include "visSetupCallback.hpp"


namespace raisim {

class ENVIRONMENT : public RaisimGymEnv {



public:

explicit ENVIRONMENT(const std::string& resourceDir, const YAML::Node& cfg, bool visualizable) :
RaisimGymEnv(resourceDir, cfg), distribution_(0.0, 0.2), visualizable_(visualizable) {

/// add objects
anymal_ = world_->addArticulatedSystem(resourceDir_+"/urdf/anymal.urdf");
anymal_->setControlMode(raisim::ControlMode::PD_PLUS_FEEDFORWARD_TORQUE);
mabi_ = world_->addArticulatedSystem(resourceDir_+"/urdf/mabi.urdf");
mabi_->setControlMode(raisim::ControlMode::PD_PLUS_FEEDFORWARD_TORQUE);
auto ground = world_->addGround();
world_->setERP(0,0);

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

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

/// this is nominal configuration of anymal
gc_init_ << 0, 0, 0.54, 1.0, 0.0, 0.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;
gc_init_.setZero(gcDim_);

/// set pd gains
Eigen::VectorXd jointPgain(gvDim_), jointDgain(gvDim_);
jointPgain.setZero(); jointPgain.tail(nJoints_).setConstant(40.0);
jointDgain.setZero(); jointDgain.tail(nJoints_).setConstant(1.0);
anymal_->setPdGains(jointPgain, jointDgain);
anymal_->setGeneralizedForce(Eigen::VectorXd::Zero(gvDim_));
mabi_->setPdGains(jointPgain, jointDgain);
mabi_->setGeneralizedForce(Eigen::VectorXd::Zero(gvDim_));

/// MUST BE DONE FOR ALL ENVIRONMENTS
obDim_ = 34; /// convention described on top
obDim_ = 1; /// convention described on top
actionDim_ = nJoints_;
actionMean_.setZero(actionDim_); actionStd_.setZero(actionDim_);
obMean_.setZero(obDim_); obStd_.setZero(obDim_);
actionMean_.setZero(actionDim_);
actionStd_.setZero(actionDim_);
obMean_.setZero(obDim_);
obStd_.setZero(obDim_);

/// action & observation scaling
actionMean_ = gc_init_.tail(nJoints_);
actionStd_.setConstant(0.6);

obMean_ << 0.44, /// average height
0.0, 0.0, 0.0, /// gravity axis 3
gc_init_.tail(12), /// joint position 12
Eigen::VectorXd::Constant(6, 0.0), /// body lin/ang vel 6
Eigen::VectorXd::Constant(12, 0.0); /// joint vel history

obStd_ << 0.12, /// average height
Eigen::VectorXd::Constant(3, 0.7), /// gravity axes angles
Eigen::VectorXd::Constant(12, 1.0 / 1.0), /// joint angles
Eigen::VectorXd::Constant(3, 2.0), /// linear velocity
Eigen::VectorXd::Constant(3, 4.0), /// angular velocities
Eigen::VectorXd::Constant(12, 10.0); /// joint velocities
obMean_.setZero(obDim_);
obStd_.setZero(obDim_);

/// Reward coefficients
READ_YAML(double, forwardVelRewardCoeff_, cfg["forwardVelRewardCoeff"])
READ_YAML(double, torqueRewardCoeff_, cfg["torqueRewardCoeff"])

gui::rewardLogger.init({"forwardVelReward", "torqueReward"});

/// 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_) {
auto vis = raisim::OgreVis::get();

/// these method must be called before initApp
vis->setWorld(world_.get());
vis->setWindowSize(1280, 720);
@@ -130,23 +119,26 @@ class ENVIRONMENT : public RaisimGymEnv {
vis->setKeyboardCallback(raisimKeyboardCallback);
vis->setSetUpCallback(setupCallback);
vis->setAntiAliasing(2);

std::cout << "Here2" << std::endl;
/// starts visualizer thread
vis->initApp();

anymalVisual_ = vis->createGraphicalObject(anymal_, "ANYmal");
std::cout << "Here20" << std::endl;
mabiVisual_ = vis->createGraphicalObject(mabi_, "mabi");
std::cout << "Here 21" << std::endl;
vis->createGraphicalObject(ground, 20, "floor", "checkerboard_green");
std::cout << "Here22" << std::endl;
desired_fps_ = 60.;
vis->setDesiredFPS(desired_fps_);
}
}
std::cout << "Constructed" << std::endl;
} // end constructor

~ENVIRONMENT() final = default;

void init() final { }

void reset() final {
anymal_->setState(gc_init_, gv_init_);
mabi_->setState(gc_init_, gv_init_);
updateObservation();
if(visualizable_)
gui::rewardLogger.clean();
@@ -159,7 +151,7 @@ class ENVIRONMENT : public RaisimGymEnv {
pTarget12_ += actionMean_;
pTarget_.tail(nJoints_) = pTarget12_;

anymal_->setPdTarget(pTarget_, vTarget_);
mabi_->setPdTarget(pTarget_, vTarget_);
auto loopCount = int(control_dt_ / simulation_dt_ + 1e-10);
auto visDecimation = int(1. / (desired_fps_ * simulation_dt_) + 1e-10);

@@ -174,7 +166,7 @@ class ENVIRONMENT : public RaisimGymEnv {

updateObservation();

torqueReward_ = torqueRewardCoeff_ * anymal_->getGeneralizedForce().squaredNorm();
torqueReward_ = torqueRewardCoeff_ * mabi_->getGeneralizedForce().squaredNorm();
forwardVelReward_ = forwardVelRewardCoeff_ * bodyLinearVel_[0];

if(visualizeThisStep_) {
@@ -184,7 +176,7 @@ class ENVIRONMENT : public RaisimGymEnv {
/// reset camera
auto vis = raisim::OgreVis::get();

vis->select(anymalVisual_->at(0), false);
vis->select(mabiVisual_->at(0), false);
vis->getCameraMan()->setYawPitchDist(Ogre::Radian(3.14), Ogre::Radian(-1.3), 3, true);
}

@@ -197,31 +189,31 @@ class ENVIRONMENT : public RaisimGymEnv {
}

void updateObservation() {
anymal_->getState(gc_, gv_);
obDouble_.setZero(obDim_); obScaled_.setZero(obDim_);

/// body height
obDouble_[0] = gc_[2];

/// body orientation
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);
obDouble_.segment(1, 3) = rot.e().row(2);

/// joint angles
obDouble_.segment(4, 12) = gc_.tail(12);

/// body velocities
bodyLinearVel_ = rot.e().transpose() * gv_.segment(0, 3);
bodyAngularVel_ = rot.e().transpose() * gv_.segment(3, 3);
obDouble_.segment(16, 3) = bodyLinearVel_;
obDouble_.segment(19, 3) = bodyAngularVel_;

/// joint velocities
obDouble_.tail(12) = gv_.tail(12);
obScaled_ = (obDouble_-obMean_).cwiseQuotient(obStd_);
mabi_->getState(gc_, gv_);
// obDouble_.setZero(obDim_); obScaled_.setZero(obDim_);
//
// /// body height
// obDouble_[0] = gc_[2];
//
// /// body orientation
// 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);
// obDouble_.segment(1, 3) = rot.e().row(2);
//
// /// joint angles
// obDouble_.segment(4, 12) = gc_.tail(12);
//
// /// body velocities
// bodyLinearVel_ = rot.e().transpose() * gv_.segment(0, 3);
// bodyAngularVel_ = rot.e().transpose() * gv_.segment(3, 3);
// obDouble_.segment(16, 3) = bodyLinearVel_;
// obDouble_.segment(19, 3) = bodyAngularVel_;
//
// /// joint velocities
// obDouble_.tail(12) = gv_.tail(12);
// obScaled_ = (obDouble_-obMean_).cwiseQuotient(obStd_);
}

void observe(Eigen::Ref<EigenVec> ob) final {
@@ -232,12 +224,6 @@ class ENVIRONMENT : public RaisimGymEnv {
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;
}
@@ -253,8 +239,8 @@ class ENVIRONMENT : public RaisimGymEnv {
int gcDim_, gvDim_, nJoints_;
bool visualizable_ = false;
std::normal_distribution<double> distribution_;
raisim::ArticulatedSystem* anymal_;
std::vector<GraphicObject> * anymalVisual_;
raisim::ArticulatedSystem* mabi_;
std::vector<GraphicObject> * mabiVisual_;
Eigen::VectorXd gc_init_, gv_init_, gc_, gv_, pTarget_, pTarget12_, vTarget_, torque_;
double terminalRewardCoeff_ = -10.;
double forwardVelRewardCoeff_ = 0., forwardVelReward_ = 0.;
@@ -264,7 +250,6 @@ class ENVIRONMENT : public RaisimGymEnv {
Eigen::VectorXd actionMean_, actionStd_, obMean_, obStd_;
Eigen::VectorXd obDouble_, obScaled_;
Eigen::Vector3d bodyLinearVel_, bodyAngularVel_;
std::set<size_t> footIndices_;
};

}
2 changes: 1 addition & 1 deletion raisim_gym/env/env/mabi/resources/default_cfg.yaml
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
seed: 1
seed: 6546984
record_video: yes

environment:
Loading

0 comments on commit fc4bde1

Please sign in to comment.