Skip to content

Commit

Permalink
[Gym] Generic locomotion envs - PART X (#204)
Browse files Browse the repository at this point in the history 
* [core] Fix various segfault in case of exceptions. More verbose error message when failing to load URDF model.
* [core] Enable to postpone Controller def after engine init, and to change it between simu.
* [python] Remove meshes and use primitives for toys models.
* [python] Refactor of engine/simulator classes to be more versatile yet simpler.
* [python/Viewer] More robust handling of meshcat comms in Jupyter.

Co-authored-by: Alexis Duburcq <alexis.duburcq@wandercraft.eu>
  • Loading branch information
@duburcqa
duburcqa and Alexis Duburcq authored Oct 1, 2020
1 parent 452ab2a commit fa175ca
Show file tree
Hide file tree
Showing 54 changed files with 3,057 additions and 55,579 deletions.
4 changes: 2 additions & 2 deletions core/include/jiminy/core/control/AbstractController.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -258,7 +258,7 @@ namespace jiminy
/// use.
///
///////////////////////////////////////////////////////////////////////////////////////////////
bool_t getIsInitialized(void) const;
bool_t const & getIsInitialized(void) const;

///////////////////////////////////////////////////////////////////////////////////////////////
///
Expand All @@ -268,7 +268,7 @@ namespace jiminy
/// initialized.
///
///////////////////////////////////////////////////////////////////////////////////////////////
bool_t getIsTelemetryConfigured(void) const;
bool_t const & getIsTelemetryConfigured(void) const;

public:
std::unique_ptr<controllerOptions_t const> baseControllerOptions_; ///< Structure with the parameters of the controller
Expand Down
13 changes: 8 additions & 5 deletions core/include/jiminy/core/engine/Engine.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,10 @@ namespace jiminy
hresult_t initialize(std::shared_ptr<Robot> robot,
std::shared_ptr<AbstractController> controller,
callbackFunctor_t callbackFct);
hresult_t initialize(std::shared_ptr<Robot> robot,
callbackFunctor_t callbackFct);

hresult_t setController(std::shared_ptr<AbstractController> controller);

/// \brief Reset the engine and compute initial state.
///
Expand Down Expand Up @@ -53,16 +57,15 @@ namespace jiminy
forceProfileFunctor_t forceFct);

bool_t const & getIsInitialized(void) const;
Robot const & getRobot(void) const;
std::shared_ptr<Robot> getRobot(void);
AbstractController const & getController(void) const;
std::shared_ptr<AbstractController> getController(void);
systemState_t const & getSystemState(void) const;
hresult_t getRobot(std::shared_ptr<Robot> & robot);
hresult_t getController(std::shared_ptr<AbstractController> & controller);
hresult_t getSystemState(systemState_t const * & systemState) const;

private:
// Make private some methods to deter their use
using EngineMultiRobot::addSystem;
using EngineMultiRobot::removeSystem;
using EngineMultiRobot::setController;
using EngineMultiRobot::addCouplingForce;
using EngineMultiRobot::removeCouplingForces;
using EngineMultiRobot::start;
Expand Down
9 changes: 8 additions & 1 deletion core/include/jiminy/core/engine/EngineMultiRobot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -487,8 +487,14 @@ namespace jiminy
std::shared_ptr<Robot> robot,
std::shared_ptr<AbstractController> controller,
callbackFunctor_t callbackFct);
hresult_t addSystem(std::string const & systemName,
std::shared_ptr<Robot> robot,
callbackFunctor_t callbackFct);
hresult_t removeSystem(std::string const & systemName);

hresult_t setController(std::string const & systemName,
std::shared_ptr<AbstractController> controller);

/// \brief Add a force linking both systems together
///
/// \details This function registers a callback function forceFct that links
Expand Down Expand Up @@ -584,7 +590,8 @@ namespace jiminy
systemDataHolder_t const * & system) const;
hresult_t getSystem(std::string const & systemName,
systemDataHolder_t * & system);
systemState_t const & getSystemState(std::string const & systemName) const;
hresult_t getSystemState(std::string const & systemName,
systemState_t const * & systemState) const;
stepperState_t const & getStepperState(void) const;
bool_t const & getIsSimulationRunning(void) const;

Expand Down
4 changes: 2 additions & 2 deletions core/src/control/AbstractController.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -227,12 +227,12 @@ namespace jiminy
baseControllerOptions_ = std::make_unique<controllerOptions_t const>(ctrlOptionsHolder_);
}

bool_t AbstractController::getIsInitialized(void) const
bool_t const & AbstractController::getIsInitialized(void) const
{
return isInitialized_;
}

bool_t AbstractController::getIsTelemetryConfigured(void) const
bool_t const & AbstractController::getIsTelemetryConfigured(void) const
{
return isTelemetryConfigured_;
}
Expand Down
80 changes: 65 additions & 15 deletions core/src/engine/Engine.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ namespace jiminy

if (returnCode == hresult_t::SUCCESS)
{
// Get some proxies
// Get some convenience proxies
robot_ = systemsDataHolder_.begin()->robot.get();
controller_ = systemsDataHolder_.begin()->controller.get();

Expand All @@ -42,6 +42,33 @@ namespace jiminy
return returnCode;
}

hresult_t Engine::initialize(std::shared_ptr<Robot> robot,
callbackFunctor_t callbackFct)
{
hresult_t returnCode = hresult_t::SUCCESS;

/* Add the system without associated name, since
it is irrelevant for a single robot engine. */
returnCode = addSystem("", std::move(robot), std::move(callbackFct));

if (returnCode == hresult_t::SUCCESS)
{
// Get some convenience proxies
robot_ = systemsDataHolder_.begin()->robot.get();
controller_ = systemsDataHolder_.begin()->controller.get();

// Set the initialization flag
isInitialized_ = true;
}

return returnCode;
}

hresult_t Engine::setController(std::shared_ptr<AbstractController> controller)
{
return setController("", controller);
}

hresult_t Engine::start(vectorN_t const & xInit,
bool_t const & isStateTheoretical,
bool_t const & resetRandomNumbers,
Expand Down Expand Up @@ -133,28 +160,51 @@ namespace jiminy
return isInitialized_;
}

Robot const & Engine::getRobot(void) const
hresult_t Engine::getRobot(std::shared_ptr<Robot> & robot)
{
return *robot_;
}
if (!isInitialized_)
{
std::cout << "Error - Engine::getRobot - The engine is not initialized." << std::endl;
return hresult_t::ERROR_BAD_INPUT;
}

std::shared_ptr<Robot> Engine::getRobot(void)
{
return systemsDataHolder_.begin()->robot;
}
robot = systemsDataHolder_.begin()->robot;

AbstractController const & Engine::getController(void) const
{
return *controller_;
return hresult_t::SUCCESS;
}

std::shared_ptr<AbstractController> Engine::getController(void)
hresult_t Engine::getController(std::shared_ptr<AbstractController> & controller)
{
return systemsDataHolder_.begin()->controller;
if (!isInitialized_)
{
std::cout << "Error - Engine::getRobot - The engine is not initialized." << std::endl;
return hresult_t::ERROR_BAD_INPUT;
}

controller = systemsDataHolder_.begin()->controller;

return hresult_t::SUCCESS;
}

systemState_t const & Engine::getSystemState(void) const
hresult_t Engine::getSystemState(systemState_t const * & systemState) const
{
return EngineMultiRobot::getSystemState("");
static systemState_t const systemStateEmpty;

hresult_t returnCode = hresult_t::SUCCESS;

if (!isInitialized_)
{
std::cout << "Error - Engine::getRobot - The engine is not initialized." << std::endl;
returnCode = hresult_t::ERROR_BAD_INPUT;
}

if (returnCode == hresult_t::SUCCESS)
{
EngineMultiRobot::getSystemState("", systemState); // It cannot fail at this point
return returnCode;
}

systemState = &systemStateEmpty;
return returnCode;
}
}
99 changes: 89 additions & 10 deletions core/src/engine/EngineMultiRobot.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
#include "jiminy/core/robot/AbstractSensor.h"
#include "jiminy/core/robot/Robot.h"
#include "jiminy/core/control/AbstractController.h"
#include "jiminy/core/control/ControllerFunctor.h"
#include "jiminy/core/Utilities.h"
#include "jiminy/core/Constants.h"

Expand Down Expand Up @@ -71,8 +72,11 @@ namespace jiminy
forcesImpulseBreakNextIt(),
forcesImpulseActive()
{
state.initialize(robot.get());
statePrev.initialize(robot.get());
if (robot)
{
state.initialize(robot.get());
statePrev.initialize(robot.get());
}
}

systemDataHolder_t::systemDataHolder_t(void) :
Expand All @@ -81,7 +85,7 @@ namespace jiminy
vectorN_t const & q,
vectorN_t const & v) -> bool_t
{
return true;
return false;
})
{
// Empty on purpose.
Expand Down Expand Up @@ -149,6 +153,67 @@ namespace jiminy
return hresult_t::SUCCESS;
}

hresult_t EngineMultiRobot::addSystem(std::string const & systemName,
std::shared_ptr<Robot> robot,
callbackFunctor_t callbackFct)
{
if (!robot->getIsInitialized())
{
std::cout << "Error - EngineMultiRobot::initialize - Robot not initialized." << std::endl;
return hresult_t::ERROR_INIT_FAILED;
}

// Create and initialize a controller doing nothing
auto setZeroFunctor = [](float64_t const & t,
vectorN_t const & q,
vectorN_t const & v,
sensorsDataMap_t const & sensorsData,
vectorN_t & u)
{
u.setZero();
};
auto controller = std::make_shared<ControllerFunctor<
decltype(setZeroFunctor), decltype(setZeroFunctor)> >(setZeroFunctor, setZeroFunctor);
controller->initialize(robot.get());

return addSystem(systemName, robot, controller, std::move(callbackFct));
}

hresult_t EngineMultiRobot::setController(std::string const & systemName,
std::shared_ptr<AbstractController> controller)
{
// Make sure that no simulation is running
if (isSimulationRunning_)
{
std::cout << "Error - EngineMultiRobot::setController - A simulation is already running. Stop it before setting a new controller for a system." << std::endl;
return hresult_t::ERROR_GENERIC;
}

// Make sure that the controller is initialized
if (!controller->getIsInitialized())
{
std::cout << "Error - EngineMultiRobot::setController - Controller not initialized." << std::endl;
return hresult_t::ERROR_INIT_FAILED;
}

// Make sure that the system for which to set the controller exists
auto systemIt = std::find_if(systemsDataHolder_.begin(), systemsDataHolder_.end(),
[&systemName](auto const & sys)
{
return (sys.name == systemName);
});
if (systemIt == systemsDataHolder_.end())
{
std::cout << "Error - EngineMultiRobot::setController - No system with this name has been added to the engine." << std::endl;
return hresult_t::ERROR_BAD_INPUT;
}

// Set the controller
systemIt->controller = controller;

return hresult_t::SUCCESS;
}

hresult_t EngineMultiRobot::removeSystem(std::string const & systemName)
{
auto systemIt = std::find_if(systemsDataHolder_.begin(), systemsDataHolder_.end(),
Expand Down Expand Up @@ -1692,6 +1757,10 @@ namespace jiminy
hresult_t EngineMultiRobot::getSystem(std::string const & systemName,
systemDataHolder_t const * & system) const
{
static systemDataHolder_t const systemEmpty;

hresult_t returnCode = hresult_t::SUCCESS;

auto systemIt = std::find_if(systemsDataHolder_.begin(), systemsDataHolder_.end(),
[&systemName](auto const & sys)
{
Expand All @@ -1700,12 +1769,17 @@ namespace jiminy
if (systemIt == systemsDataHolder_.end())
{
std::cout << "Error - EngineMultiRobot::getSystem - No system with this name has been added to the engine." << std::endl;
return hresult_t::ERROR_BAD_INPUT;
returnCode = hresult_t::ERROR_BAD_INPUT;
}

system = &(*systemIt);
if (returnCode == hresult_t::SUCCESS)
{
system = &(*systemIt);
return returnCode;
}

return hresult_t::SUCCESS;
system = &systemEmpty;
return returnCode;
}

hresult_t EngineMultiRobot::getSystem(std::string const & systemName,
Expand All @@ -1723,18 +1797,23 @@ namespace jiminy
return returnCode;
}

systemState_t const & EngineMultiRobot::getSystemState(std::string const & systemName) const
hresult_t EngineMultiRobot::getSystemState(std::string const & systemName,
systemState_t const * & systemState) const
{
static systemState_t const systemStateEmpty;

hresult_t returnCode = hresult_t::SUCCESS;

systemDataHolder_t const * system;
hresult_t returnCode = getSystem(systemName, system);
returnCode = getSystem(systemName, system);
if (returnCode == hresult_t::SUCCESS)
{
return system->state;
systemState = &(system->state);
return returnCode;
}

return systemStateEmpty;
systemState = &systemStateEmpty;
return returnCode;
}

stepperState_t const & EngineMultiRobot::getStepperState(void) const
Expand Down
12 changes: 11 additions & 1 deletion core/src/robot/Model.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -998,13 +998,23 @@ namespace jiminy
{
pinocchio::urdf::buildModel(urdfPath, pncModel_);
}
}
catch (std::exception& e)
{
std::cout << "Error - Model::loadUrdfModel - Something is wrong with the URDF. Impossible to build a model from it.\n"
<< "Raised by exception: " << e.what() << std::endl;
return hresult_t::ERROR_BAD_INPUT;
}

try
{
// Build robot geometry model
pinocchio::urdf::buildGeom(pncModel_, urdfPath, pinocchio::COLLISION, pncGeometryModel_, meshPackageDirs);
}
catch (std::exception& e)
{
std::cout << "Error - Model::loadUrdfModel - Something is wrong with the URDF. Impossible to build a model from it." << std::endl;
std::cout << "Error - Model::loadUrdfModel - Something is wrong with the URDF. Impossible to load the collision geometries.\n"
<< "Raised by exception: " << e.what() << std::endl;
return hresult_t::ERROR_BAD_INPUT;
}

Expand Down
Loading

0 comments on commit fa175ca

Please sign in to comment.