Skip to content

Commit

Permalink
raisim train can success sometimes, but always failed
Browse files Browse the repository at this point in the history
  • Loading branch information
@legubiao
legubiao committed Aug 29, 2024
1 parent 305a3ef commit 60026f2
Show file tree
Hide file tree
Showing 80 changed files with 5,546 additions and 5,747 deletions.
2 changes: 0 additions & 2 deletions README.md
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,4 @@
# OCS2_ROS2 Toolbox
<script src="//unpkg.com/docsify-bilibili/bilibili.min.js"></script>

## 1. Summary
OCS2_ROS2 is developed based on [OCS2](https://github.com/leggedrobotics/ocs2), it was refactored to be compatible with ROS2 and modern cmake. Below is the current todolist of the project:

Expand Down
6 changes: 3 additions & 3 deletions ...et/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ namespace ocs2::legged_robot {
/**
* Legged robot MPC-Net interface between C++ and Python.
*/
class LeggedRobotMpcnetInterface final : public ocs2::mpcnet::MpcnetInterfaceBase {
class LeggedRobotMpcnetInterface final : public mpcnet::MpcnetInterfaceBase {
public:
/**
* Constructor.
Expand All @@ -61,8 +61,8 @@ namespace ocs2::legged_robot {
static std::unique_ptr<MPC_BASE> getMpc(const LeggedRobotInterface &leggedRobotInterface);

// Legged robot interface pointers (keep alive for Pinocchio interface)
std::vector<std::unique_ptr<LeggedRobotInterface> > leggedRobotInterfacePtrs_;
std::vector<std::unique_ptr<LeggedRobotInterface> > interfaces_;
// Legged robot RaiSim conversions pointers (keep alive for RaiSim rollout)
std::vector<std::unique_ptr<LeggedRobotRaisimConversions> > leggedRobotRaisimConversionsPtrs_;
std::vector<std::unique_ptr<LeggedRobotRaisimConversions> > conversions_ptrs;
};
}
6 changes: 3 additions & 3 deletions ...es/ocs2_mpcnet/ocs2_legged_robot_mpcnet/ocs2_legged_robot_mpcnet/config/legged_robot.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,7 @@ EPSILON: 1.e-8
# whether to cheat by adding the gating loss
CHEATING: True
# parameter to control the relative importance of both loss types
LAMBDA: 1.0
LAMBDA: 1.5
# dictionary for the gating loss (assigns modes to experts responsible for the corresponding contact configuration)
EXPERT_FOR_MODE:
6: 1 # trot
Expand Down Expand Up @@ -235,6 +235,6 @@ POLICY_EVALUATION_TASKS: 3
#
BATCH_SIZE: 128
LEARNING_RATE: 1.e-3
LEARNING_ITERATIONS: 100000
LEARNING_ITERATIONS: 200000
GRADIENT_CLIPPING: True
GRADIENT_CLIPPING_VALUE: 1.0
GRADIENT_CLIPPING_VALUE: 0.5
2 changes: 1 addition & 1 deletion advance examples/ocs2_mpcnet/ocs2_legged_robot_mpcnet/ocs2_legged_robot_mpcnet/mpcnet.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -194,7 +194,7 @@ def get_random_target_state_trot(self) -> np.ndarray:
return np.array(self.config.DEFAULT_TARGET_STATE) + random_deviation

def get_tasks(
self, tasks_number: int, duration: float
self, tasks_number: int, duration: float
) -> Tuple[SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray]:
"""Get tasks.
Expand Down
62 changes: 35 additions & 27 deletions advance examples/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,60 +62,68 @@ namespace ocs2::legged_robot {
std::vector<std::unique_ptr<RolloutBase> > rolloutPtrs;
std::vector<std::shared_ptr<mpcnet::MpcnetDefinitionBase> > mpcnetDefinitionPtrs;
std::vector<std::shared_ptr<ReferenceManagerInterface> > referenceManagerPtrs;

mpcPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads);
mpcnetPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads);
rolloutPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads);
mpcnetDefinitionPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads);
referenceManagerPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads);

for (int i = 0; i < (nDataGenerationThreads + nPolicyEvaluationThreads); i++) {
leggedRobotInterfacePtrs_.push_back(
interfaces_.push_back(
std::make_unique<LeggedRobotInterface>(taskFile, urdfFile, referenceFile));
auto mpcnetDefinitionPtr = std::make_shared<LeggedRobotMpcnetDefinition>(*leggedRobotInterfacePtrs_[i]);
mpcPtrs.push_back(getMpc(*leggedRobotInterfacePtrs_[i]));

auto definition = std::make_shared<LeggedRobotMpcnetDefinition>(*interfaces_[i]);
mpcPtrs.push_back(getMpc(*interfaces_[i]));
mpcnetPtrs.push_back(std::make_unique<mpcnet::MpcnetOnnxController>(
mpcnetDefinitionPtr, leggedRobotInterfacePtrs_[i]->getReferenceManagerPtr(),
definition, interfaces_[i]->getReferenceManagerPtr(),
onnxEnvironmentPtr));

if (raisim) {
RaisimRolloutSettings raisimRolloutSettings(raisimFile, "rollout");
raisimRolloutSettings.portNumber_ += i;
leggedRobotRaisimConversionsPtrs_.push_back(std::make_unique<LeggedRobotRaisimConversions>(
leggedRobotInterfacePtrs_[i]->getPinocchioInterface(),
leggedRobotInterfacePtrs_[i]->getCentroidalModelInfo(),
leggedRobotInterfacePtrs_[i]->getInitialState()));
leggedRobotRaisimConversionsPtrs_[i]->loadSettings(raisimFile, "rollout", true);
RaisimRolloutSettings rollout_settings(raisimFile, "rollout");
rollout_settings.portNumber_ += i;

conversions_ptrs.push_back(std::make_unique<LeggedRobotRaisimConversions>(
interfaces_[i]->getPinocchioInterface(),
interfaces_[i]->getCentroidalModelInfo(),
interfaces_[i]->getInitialState()));

conversions_ptrs[i]->loadSettings(raisimFile, "rollout", true);
rolloutPtrs.push_back(std::make_unique<RaisimRollout>(
urdfFile, resourcePath,
[&, i](const vector_t &state, const vector_t &input) {
return leggedRobotRaisimConversionsPtrs_[i]->stateToRaisimGenCoordGenVel(state, input);
return conversions_ptrs[i]->stateToRaisimGenCoordGenVel(state, input);
},
[&, i](const Eigen::VectorXd &q, const Eigen::VectorXd &dq) {
return leggedRobotRaisimConversionsPtrs_[i]->raisimGenCoordGenVelToState(q, dq);
return conversions_ptrs[i]->raisimGenCoordGenVelToState(q, dq);
},
[&, i](double time, const vector_t &input, const vector_t &state, const Eigen::VectorXd &q,
const Eigen::VectorXd &dq) {
return leggedRobotRaisimConversionsPtrs_[i]->inputToRaisimGeneralizedForce(
return conversions_ptrs[i]->inputToRaisimGeneralizedForce(
time, input, state, q, dq);
},
nullptr, raisimRolloutSettings,
nullptr, rollout_settings,
[&, i](double time, const vector_t &input, const vector_t &state, const Eigen::VectorXd &q,
const Eigen::VectorXd &dq) {
return leggedRobotRaisimConversionsPtrs_[i]->inputToRaisimPdTargets(
return conversions_ptrs[i]->inputToRaisimPdTargets(
time, input, state, q, dq);
}));
if (raisimRolloutSettings.generateTerrain_) {
raisim::TerrainProperties terrainProperties;
terrainProperties.zScale = raisimRolloutSettings.terrainRoughness_;
terrainProperties.seed = raisimRolloutSettings.terrainSeed_ + i;

if (rollout_settings.generateTerrain_) {
raisim::TerrainProperties properties;
properties.zScale = rollout_settings.terrainRoughness_;
properties.seed = rollout_settings.terrainSeed_ + i;
properties.heightOffset = -0.4;
auto terrainPtr = dynamic_cast<RaisimRollout *>(rolloutPtrs[i].get())->generateTerrain(
terrainProperties);
leggedRobotRaisimConversionsPtrs_[i]->setTerrain(*terrainPtr);
properties);
conversions_ptrs[i]->setTerrain(*terrainPtr);
}
} else {
rolloutPtrs.push_back(
std::unique_ptr<RolloutBase>(leggedRobotInterfacePtrs_[i]->getRollout().clone()));
std::unique_ptr<RolloutBase>(interfaces_[i]->getRollout().clone()));
}
mpcnetDefinitionPtrs.push_back(mpcnetDefinitionPtr);
referenceManagerPtrs.push_back(leggedRobotInterfacePtrs_[i]->getReferenceManagerPtr());
mpcnetDefinitionPtrs.push_back(definition);
referenceManagerPtrs.push_back(interfaces_[i]->getReferenceManagerPtr());
}
mpcnetRolloutManagerPtr_ = std::make_unique<mpcnet::MpcnetRolloutManager>(
nDataGenerationThreads, nPolicyEvaluationThreads,
Expand All @@ -126,13 +134,13 @@ namespace ocs2::legged_robot {

std::unique_ptr<MPC_BASE> LeggedRobotMpcnetInterface::getMpc(const LeggedRobotInterface &leggedRobotInterface) {
// ensure MPC and DDP settings are as needed for MPC-Net
const auto mpcSettings = [&]() {
const auto mpcSettings = [&] {
auto settings = leggedRobotInterface.mpcSettings();
settings.debugPrint_ = false;
settings.coldStart_ = false;
return settings;
}();
const auto ddpSettings = [&]() {
const auto ddpSettings = [&] {
auto settings = leggedRobotInterface.ddpSettings();
settings.algorithm_ = ddp::Algorithm::SLQ;
settings.nThreads_ = 1;
Expand Down
3 changes: 1 addition & 2 deletions advance examples/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetBehavioralController.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,9 +27,8 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
******************************************************************************/

#include "ocs2_mpcnet_core/control/MpcnetBehavioralController.h"

#include <ocs2_core/misc/Numerics.h>
#include <ocs2_mpcnet_core/control/MpcnetBehavioralController.h>


namespace ocs2::mpcnet {
Expand Down
10 changes: 5 additions & 5 deletions advance examples/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -189,7 +189,7 @@ namespace ocs2::mpcnet {
const auto result = policyEvaluationPtrs_[threadNumber]->run(
alpha, policyFilePath, timeStep, initialObservations.at(i),
modeSchedules.at(i), targetTrajectories.at(i));
nPolicyEvaluationTasksDone_++;
++nPolicyEvaluationTasksDone_;
// print thread and task number
std::cerr << "Policy evaluation thread " << threadNumber << " finished task " <<
nPolicyEvaluationTasksDone_ << "\n";
Expand All @@ -212,12 +212,12 @@ namespace ocs2::mpcnet {
// check if done
if (nPolicyEvaluationTasksDone_ < policyEvaluationFtrs_.size()) {
return false;
} else if (nPolicyEvaluationTasksDone_ == policyEvaluationFtrs_.size()) {
}
if (nPolicyEvaluationTasksDone_ == policyEvaluationFtrs_.size()) {
return true;
} else {
throw std::runtime_error(
"[MpcnetRolloutManager::isPolicyEvaluationDone] error since more tasks done than futures available.");
}
throw std::runtime_error(
"[MpcnetRolloutManager::isPolicyEvaluationDone] error since more tasks done than futures available.");
}


Expand Down
173 changes: 94 additions & 79 deletions basic examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#pragma once

// ocs2
#include <ocs2_centroidal_model/FactoryFunctions.h>
#include <ocs2_core/Types.h>
#include <ocs2_core/penalties/Penalties.h>
#include <ocs2_ddp/DDP_Settings.h>
Expand All @@ -50,81 +49,97 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
* LeggedRobotInterface class
* General interface for mpc implementation on the legged robot model
*/
namespace ocs2 {
namespace legged_robot {

class LeggedRobotInterface final : public RobotInterface {
public:
/**
* Constructor
*
* @throw Invalid argument error if input task file or urdf file does not exist.
*
* @param [in] taskFile: The absolute path to the configuration file for the MPC.
* @param [in] urdfFile: The absolute path to the URDF file for the robot.
* @param [in] referenceFile: The absolute path to the reference configuration file.
* @param [in] useHardFrictionConeConstraint: Which to use hard or soft friction cone constraints.
*/
LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile,
bool useHardFrictionConeConstraint = false);

~LeggedRobotInterface() override = default;

const OptimalControlProblem& getOptimalControlProblem() const override { return *problemPtr_; }

const ModelSettings& modelSettings() const { return modelSettings_; }
const ddp::Settings& ddpSettings() const { return ddpSettings_; }
const mpc::Settings& mpcSettings() const { return mpcSettings_; }
const rollout::Settings& rolloutSettings() const { return rolloutSettings_; }
const sqp::Settings& sqpSettings() { return sqpSettings_; }
const ipm::Settings& ipmSettings() { return ipmSettings_; }

const vector_t& getInitialState() const { return initialState_; }
const RolloutBase& getRollout() const { return *rolloutPtr_; }
PinocchioInterface& getPinocchioInterface() { return *pinocchioInterfacePtr_; }
const CentroidalModelInfo& getCentroidalModelInfo() const { return centroidalModelInfo_; }
std::shared_ptr<SwitchedModelReferenceManager> getSwitchedModelReferenceManagerPtr() const { return referenceManagerPtr_; }

const LeggedRobotInitializer& getInitializer() const override { return *initializerPtr_; }
std::shared_ptr<ReferenceManagerInterface> getReferenceManagerPtr() const override { return referenceManagerPtr_; }

private:
void setupOptimalConrolProblem(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile, bool verbose);

std::shared_ptr<GaitSchedule> loadGaitSchedule(const std::string& file, bool verbose) const;

std::unique_ptr<StateInputCost> getBaseTrackingCost(const std::string& taskFile, const CentroidalModelInfo& info, bool verbose);
matrix_t initializeInputCostWeight(const std::string& taskFile, const CentroidalModelInfo& info);

std::pair<scalar_t, RelaxedBarrierPenalty::Config> loadFrictionConeSettings(const std::string& taskFile, bool verbose) const;
std::unique_ptr<StateInputConstraint> getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient);
std::unique_ptr<StateInputCost> getFrictionConeSoftConstraint(size_t contactPointIndex, scalar_t frictionCoefficient,
const RelaxedBarrierPenalty::Config& barrierPenaltyConfig);
std::unique_ptr<StateInputConstraint> getZeroForceConstraint(size_t contactPointIndex);
std::unique_ptr<StateInputConstraint> getZeroVelocityConstraint(const EndEffectorKinematics<scalar_t>& eeKinematics,
size_t contactPointIndex, bool useAnalyticalGradients);
std::unique_ptr<StateInputConstraint> getNormalVelocityConstraint(const EndEffectorKinematics<scalar_t>& eeKinematics,
size_t contactPointIndex, bool useAnalyticalGradients);

ModelSettings modelSettings_;
ddp::Settings ddpSettings_;
mpc::Settings mpcSettings_;
sqp::Settings sqpSettings_;
ipm::Settings ipmSettings_;
const bool useHardFrictionConeConstraint_;

std::unique_ptr<PinocchioInterface> pinocchioInterfacePtr_;
CentroidalModelInfo centroidalModelInfo_;

std::unique_ptr<OptimalControlProblem> problemPtr_;
std::shared_ptr<SwitchedModelReferenceManager> referenceManagerPtr_;

rollout::Settings rolloutSettings_;
std::unique_ptr<RolloutBase> rolloutPtr_;
std::unique_ptr<LeggedRobotInitializer> initializerPtr_;

vector_t initialState_;
};

} // namespace legged_robot
} // namespace ocs2
namespace ocs2::legged_robot {
class LeggedRobotInterface final : public RobotInterface {
public:
/**
* Constructor
*
* @throw Invalid argument error if input task file or urdf file does not exist.
*
* @param [in] taskFile: The absolute path to the configuration file for the MPC.
* @param [in] urdfFile: The absolute path to the URDF file for the robot.
* @param [in] referenceFile: The absolute path to the reference configuration file.
* @param [in] useHardFrictionConeConstraint: Which to use hard or soft friction cone constraints.
*/
LeggedRobotInterface(const std::string &taskFile, const std::string &urdfFile,
const std::string &referenceFile,
bool useHardFrictionConeConstraint = false);

~LeggedRobotInterface() override = default;

const OptimalControlProblem &getOptimalControlProblem() const override { return *problemPtr_; }

const ModelSettings &modelSettings() const { return modelSettings_; }
const ddp::Settings &ddpSettings() const { return ddpSettings_; }
const mpc::Settings &mpcSettings() const { return mpcSettings_; }
const rollout::Settings &rolloutSettings() const { return rolloutSettings_; }
const sqp::Settings &sqpSettings() { return sqpSettings_; }
const ipm::Settings &ipmSettings() { return ipmSettings_; }

const vector_t &getInitialState() const { return initialState_; }
const RolloutBase &getRollout() const { return *rolloutPtr_; }
PinocchioInterface &getPinocchioInterface() { return *pinocchioInterfacePtr_; }
const CentroidalModelInfo &getCentroidalModelInfo() const { return centroidalModelInfo_; }

std::shared_ptr<SwitchedModelReferenceManager> getSwitchedModelReferenceManagerPtr() const {
return referenceManagerPtr_;
}

const LeggedRobotInitializer &getInitializer() const override { return *initializerPtr_; }

std::shared_ptr<ReferenceManagerInterface> getReferenceManagerPtr() const override {
return referenceManagerPtr_;
}

private:
void setupOptimalConrolProblem(const std::string &taskFile, const std::string &urdfFile,
const std::string &referenceFile, bool verbose);

std::shared_ptr<GaitSchedule> loadGaitSchedule(const std::string &file, bool verbose) const;

std::unique_ptr<StateInputCost> getBaseTrackingCost(const std::string &taskFile,
const CentroidalModelInfo &info, bool verbose);

matrix_t initializeInputCostWeight(const std::string &taskFile, const CentroidalModelInfo &info);

std::pair<scalar_t, RelaxedBarrierPenalty::Config> loadFrictionConeSettings(
const std::string &taskFile, bool verbose) const;

std::unique_ptr<StateInputConstraint> getFrictionConeConstraint(
size_t contactPointIndex, scalar_t frictionCoefficient);

std::unique_ptr<StateInputCost> getFrictionConeSoftConstraint(
size_t contactPointIndex, scalar_t frictionCoefficient,
const RelaxedBarrierPenalty::Config &barrierPenaltyConfig);

std::unique_ptr<StateInputConstraint> getZeroForceConstraint(size_t contactPointIndex);

std::unique_ptr<StateInputConstraint> getZeroVelocityConstraint(
const EndEffectorKinematics<scalar_t> &eeKinematics,
size_t contactPointIndex, bool useAnalyticalGradients);

std::unique_ptr<StateInputConstraint> getNormalVelocityConstraint(
const EndEffectorKinematics<scalar_t> &eeKinematics,
size_t contactPointIndex, bool useAnalyticalGradients);

ModelSettings modelSettings_;
ddp::Settings ddpSettings_;
mpc::Settings mpcSettings_;
sqp::Settings sqpSettings_;
ipm::Settings ipmSettings_;
const bool useHardFrictionConeConstraint_;

std::unique_ptr<PinocchioInterface> pinocchioInterfacePtr_;
CentroidalModelInfo centroidalModelInfo_;

std::unique_ptr<OptimalControlProblem> problemPtr_;
std::shared_ptr<SwitchedModelReferenceManager> referenceManagerPtr_;

rollout::Settings rolloutSettings_;
std::unique_ptr<RolloutBase> rolloutPtr_;
std::unique_ptr<LeggedRobotInitializer> initializerPtr_;

vector_t initialState_;
};
}
Loading

0 comments on commit 60026f2

Please sign in to comment.