Skip to content

Commit

Permalink
[core] Consistent telemetry data. (#231)
Browse files Browse the repository at this point in the history 
* [core] Enable to specify the initial acceleration optionally.
* [core] Log command instead of motors efforts at engine level.
[core] Fixes hasDynamicsChanged not being set to false after recomputing the dynamics for FSAL stepper hypothesis.
* [core] Fetch telemetry right after updating command but before integrating the dynamics.
[core] Log data at the min(controller_period, sensors_period) if > 0, every internal steps otherwise
* [gym] Fix argument forwarding with multiple inheritence.
* [gym] Add pipeline design unit tests.

Co-authored-by: Alexis Duburcq <[email protected]>
  • Loading branch information
@duburcqa
duburcqa and Alexis Duburcq authored Nov 24, 2020
1 parent ef3ffee commit 7d21b0a
Show file tree
Hide file tree
Showing 24 changed files with 599 additions and 209 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
cmake_minimum_required(VERSION 3.10)

# Set the build version
set(BUILD_VERSION 1.4.13)
set(BUILD_VERSION 1.4.14)

# Add definition of Jiminy version for C++ headers
add_definitions("-DJIMINY_VERSION=\"${BUILD_VERSION}\"")
Expand Down
10 changes: 8 additions & 2 deletions core/include/jiminy/core/engine/Engine.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,25 +30,31 @@ namespace jiminy
/// \details This function reset the engine, the robot and the controller, and update internal data
/// to match the given initial state.
///
/// \param[in] xInit Initial state.
/// \param[in] qInit Initial configuration.
/// \param[in] vInit Initial velocity.
/// \param[in] aInit Initial acceleration. Optional: Zero by default.
/// \param[in] isStateTheoretical Specify if the initial state is associated with the current or theoretical model
/// \param[in] resetRandomNumbers Whether or not to reset the random number generator.
/// \param[in] resetDynamicForceRegister Whether or not to register the external force profiles applied
/// during the simulation.
hresult_t start(vectorN_t const & qInit,
vectorN_t const & vInit,
std::optional<vectorN_t> const & aInit = std::nullopt,
bool_t const & isStateTheoretical = false,
bool_t const & resetRandomNumbers = false,
bool_t const & resetDynamicForceRegister = false);

/// \brief Run a simulation of duration tEnd, starting at xInit.
///
/// \param[in] tEnd End time, i.e. amount of time to simulate.
/// \param[in] xInit Initial state, i.e. state at t=0.
/// \param[in] qInit Initial configuration, i.e. state at t=0.
/// \param[in] vInit Initial velocity, i.e. state at t=0.
/// \param[in] aInit Initial acceleration, i.e. state at t=0.
/// \param[in] isStateTheoretical Specify if the initial state is associated with the current or theoretical model
hresult_t simulate(float64_t const & tEnd,
vectorN_t const & qInit,
vectorN_t const & vInit,
std::optional<vectorN_t> const & aInit = std::nullopt,
bool_t const & isStateTheoretical = false);

hresult_t registerForceImpulse(std::string const & frameName,
Expand Down
24 changes: 11 additions & 13 deletions core/include/jiminy/core/engine/EngineMultiRobot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,8 @@ namespace jiminy

void reset(float64_t const & dtInit,
std::vector<vectorN_t> const & qSplitInit,
std::vector<vectorN_t> const & vSplitInit)
std::vector<vectorN_t> const & vSplitInit,
std::vector<vectorN_t> const & aSplitInit)
{
iter = 0U;
iterFailed = 0U;
Expand All @@ -66,14 +67,7 @@ namespace jiminy
tError = 0.0;
qSplit = qSplitInit;
vSplit = vSplitInit;
aSplit.clear();
aSplit.reserve(vSplitInit.size());
std::transform(vSplitInit.begin(), vSplitInit.end(),
std::back_inserter(aSplit),
[](auto const & v) -> vectorN_t
{
return vectorN_t::Zero(v.size());
});
aSplit = aSplitInit;
}

public:
Expand Down Expand Up @@ -157,7 +151,7 @@ namespace jiminy
config["enableConfiguration"] = true;
config["enableVelocity"] = true;
config["enableAcceleration"] = true;
config["enableEffort"] = true;
config["enableCommand"] = true;
config["enableEnergy"] = true;
config["timeUnit"] = 1.0e9;
return config;
Expand Down Expand Up @@ -268,15 +262,15 @@ namespace jiminy
bool_t const enableConfiguration;
bool_t const enableVelocity;
bool_t const enableAcceleration;
bool_t const enableEffort;
bool_t const enableCommand;
bool_t const enableEnergy;
float64_t const timeUnit;

telemetryOptions_t(configHolder_t const & options) :
enableConfiguration(boost::get<bool_t>(options.at("enableConfiguration"))),
enableVelocity(boost::get<bool_t>(options.at("enableVelocity"))),
enableAcceleration(boost::get<bool_t>(options.at("enableAcceleration"))),
enableEffort(boost::get<bool_t>(options.at("enableEffort"))),
enableCommand(boost::get<bool_t>(options.at("enableCommand"))),
enableEnergy(boost::get<bool_t>(options.at("enableEnergy"))),
timeUnit(boost::get<float64_t>(options.at("timeUnit")))
{
Expand Down Expand Up @@ -366,11 +360,13 @@ namespace jiminy
///
/// \param[in] qInit Initial configuration of every system.
/// \param[in] vInit Initial velocity of every system.
/// \param[in] aInit Initial acceleration of every system. Optional: Zero by default.
/// \param[in] resetRandomNumbers Whether or not to reset the random number generator.
/// \param[in] resetDynamicForceRegister Whether or not to register the external force profiles applied
/// during the simulation.
hresult_t start(std::map<std::string, vectorN_t> const & qInit,
std::map<std::string, vectorN_t> const & vInit,
std::optional<std::map<std::string, vectorN_t> > const & aInit = std::nullopt,
bool_t const & resetRandomNumbers = false,
bool_t const & resetDynamicForceRegister = false);

Expand All @@ -397,9 +393,11 @@ namespace jiminy
/// \param[in] tEnd End time, i.e. amount of time to simulate.
/// \param[in] qInit Initial configuration of every system, i.e. at t=0.0.
/// \param[in] vInit Initial velocity of every system, i.e. at t=0.0.
/// \param[in] aInit Initial acceleration of every system, i.e. at t=0.0. Optional: Zero by default.
hresult_t simulate(float64_t const & tEnd,
std::map<std::string, vectorN_t> const & qInit,
std::map<std::string, vectorN_t> const & vInit);
std::map<std::string, vectorN_t> const & vInit,
std::optional<std::map<std::string, vectorN_t> > const & aInit = std::nullopt);

/// \brief Apply an impulse force on a frame for a given duration at the desired time.
/// The force must be given in the world frame.
Expand Down
2 changes: 1 addition & 1 deletion core/include/jiminy/core/engine/System.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,7 +138,7 @@ namespace jiminy
std::vector<std::string> positionFieldnames;
std::vector<std::string> velocityFieldnames;
std::vector<std::string> accelerationFieldnames;
std::vector<std::string> motorEffortFieldnames;
std::vector<std::string> commandFieldnames;
std::string energyFieldname;

systemState_t state; ///< Internal buffer with the state for the integration loop
Expand Down
16 changes: 8 additions & 8 deletions core/include/jiminy/core/robot/Model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -193,14 +193,14 @@ namespace jiminy
std::vector<std::string> const & getVelocityFieldnames(void) const;
std::vector<std::string> const & getAccelerationFieldnames(void) const;

hresult_t getFlexibleStateFromRigid(vectorN_t const & qRigid,
vectorN_t const & vRigid,
vectorN_t & qFlex,
vectorN_t & vFlex) const;
hresult_t getRigidStateFromFlexible(vectorN_t const & qFlex,
vectorN_t const & vFlex,
vectorN_t & qRigid,
vectorN_t & vRigid) const;
hresult_t getFlexibleConfigurationFromRigid(vectorN_t const & qRigid,
vectorN_t & qFlex) const;
hresult_t getRigidConfigurationFromFlexible(vectorN_t const & qFlex,
vectorN_t & qRigid) const;
hresult_t getFlexibleVelocityFromRigid(vectorN_t const & vRigid,
vectorN_t & vFlex) const;
hresult_t getRigidVelocityFromFlexible(vectorN_t const & vFlex,
vectorN_t & vRigid) const;

protected:
hresult_t loadUrdfModel(std::string const & urdfPath,
Expand Down
4 changes: 2 additions & 2 deletions core/include/jiminy/core/robot/Robot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -174,7 +174,7 @@ namespace jiminy
vectorN_t getEffortLimit(void) const;
vectorN_t getMotorsInertias(void) const;

std::vector<std::string> const & getMotorEffortFieldnames(void) const;
std::vector<std::string> const & getCommandFieldnames(void) const;
// Getters without 'get' prefix for consistency with pinocchio C++ API
int32_t const & nmotors(void) const;

Expand All @@ -196,7 +196,7 @@ namespace jiminy
std::unordered_map<std::string, bool_t> sensorTelemetryOptions_;
std::vector<std::string> motorsNames_; ///< Name of the motors
std::unordered_map<std::string, std::vector<std::string> > sensorsNames_; ///< Name of the sensors
std::vector<std::string> motorEffortFieldnames_; ///< Fieldnames of the efforts
std::vector<std::string> commandFieldnames_; ///< Fieldnames of the efforts
int32_t nmotors_; ///< The number of motors

std::vector<robotConstraint_t> constraintsHolder_;
Expand Down
95 changes: 67 additions & 28 deletions core/src/engine/Engine.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,8 +82,66 @@ namespace jiminy
return setController("", controller);
}

hresult_t singleToMultipleSystemsInitialData(Robot const & robot,
bool_t const & isStateTheoretical,
vectorN_t const & qInit,
vectorN_t const & vInit,
std::optional<vectorN_t> const & aInit,
std::map<std::string, vectorN_t> & qInitList,
std::map<std::string, vectorN_t> & vInitList,
std::optional<std::map<std::string, vectorN_t> > & aInitList)
{
hresult_t returnCode = hresult_t::SUCCESS;

if (isStateTheoretical && robot.mdlOptions_->dynamics.enableFlexibleModel)
{
vectorN_t q0;
returnCode = robot.getFlexibleConfigurationFromRigid(qInit, q0);
qInitList.emplace("", std::move(q0));
}
else
{
qInitList.emplace("", qInit);
}

if (returnCode == hresult_t::SUCCESS)
{
if (isStateTheoretical && robot.mdlOptions_->dynamics.enableFlexibleModel)
{
vectorN_t v0;
returnCode = robot.getFlexibleVelocityFromRigid(vInit, v0);
vInitList.emplace("", std::move(v0));
}
else
{
vInitList.emplace("", vInit);
}
}
if (returnCode == hresult_t::SUCCESS)
{
if (aInit)
{
aInitList.emplace();
if (isStateTheoretical && robot.mdlOptions_->dynamics.enableFlexibleModel)
{
vectorN_t a0;
returnCode = robot.getFlexibleVelocityFromRigid(*aInit, a0);
aInitList->emplace("", std::move(a0));
}
else
{
aInitList->emplace("", *aInit);
}
}
}

return returnCode;
}


hresult_t Engine::start(vectorN_t const & qInit,
vectorN_t const & vInit,
std::optional<vectorN_t> const & aInit,
bool_t const & isStateTheoretical,
bool_t const & resetRandomNumbers,
bool_t const & resetDynamicForceRegister)
Expand All @@ -98,27 +156,17 @@ namespace jiminy

std::map<std::string, vectorN_t> qInitList;
std::map<std::string, vectorN_t> vInitList;
std::optional<std::map<std::string, vectorN_t> > aInitList = std::nullopt;
if (returnCode == hresult_t::SUCCESS)
{
if (isStateTheoretical && robot_->mdlOptions_->dynamics.enableFlexibleModel)
{
vectorN_t q0;
vectorN_t v0;
returnCode = robot_->getFlexibleStateFromRigid(qInit, vInit, q0, v0);
qInitList.emplace("", std::move(q0));
vInitList.emplace("", std::move(v0));
}
else
{
qInitList.emplace("", std::move(qInit));
vInitList.emplace("", std::move(vInit));
}
returnCode = singleToMultipleSystemsInitialData(
*robot_, isStateTheoretical, qInit, vInit, aInit, qInitList, vInitList, aInitList);
}

if (returnCode == hresult_t::SUCCESS)
{
returnCode = EngineMultiRobot::start(
qInitList, vInitList, resetRandomNumbers, resetDynamicForceRegister);
qInitList, vInitList, aInitList, resetRandomNumbers, resetDynamicForceRegister);
}

return returnCode;
Expand All @@ -127,6 +175,7 @@ namespace jiminy
hresult_t Engine::simulate(float64_t const & tEnd,
vectorN_t const & qInit,
vectorN_t const & vInit,
std::optional<vectorN_t> const & aInit,
bool_t const & isStateTheoretical)
{
hresult_t returnCode = hresult_t::SUCCESS;
Expand All @@ -139,26 +188,16 @@ namespace jiminy

std::map<std::string, vectorN_t> qInitList;
std::map<std::string, vectorN_t> vInitList;
std::optional<std::map<std::string, vectorN_t> > aInitList = std::nullopt;
if (returnCode == hresult_t::SUCCESS)
{
if (isStateTheoretical && robot_->mdlOptions_->dynamics.enableFlexibleModel)
{
vectorN_t q0;
vectorN_t v0;
returnCode = robot_->getFlexibleStateFromRigid(qInit, vInit, q0, v0);
qInitList.emplace("", std::move(q0));
vInitList.emplace("", std::move(v0));
}
else
{
qInitList.emplace("", std::move(qInit));
vInitList.emplace("", std::move(vInit));
}
returnCode = singleToMultipleSystemsInitialData(
*robot_, isStateTheoretical, qInit, vInit, aInit, qInitList, vInitList, aInitList);
}

if (returnCode == hresult_t::SUCCESS)
{
returnCode = EngineMultiRobot::simulate(tEnd, qInitList, vInitList);
returnCode = EngineMultiRobot::simulate(tEnd, qInitList, vInitList, aInitList);
}

return returnCode;
Expand Down
Loading

0 comments on commit 7d21b0a

Please sign in to comment.