Skip to content

Commit

Permalink
[python/viewer] Add options to display Capture Point and external for…
Browse files Browse the repository at this point in the history 
…ces. (#341)

* [core] Compute velocity of the center of mass of each subtree systematically.
* [core] Fix internal update period computation when defined control and sensor update period of the engine without robot.
* [core] Add option to register external forces to the telemetry.
* [python/robot] Add fallback if 'dynamics' fields 'damping'/'friction' are partially defined in URDF file.
* [python/processing] Add efficient convex hull tools. Improve efficiency and versatility of 'interpolate_zoh'.
* [python/log] Extract external forces from log if recorded.
* [python/viewer] Add option to display DCM in viewer.
* [python/viewer] Fix default display marker value if backend is not Panda3d.
* [python/viewer] Add option to display external linear forces at joint level in viewer. Improve efficiency of markers update.
* [python/viewer] 'jiminy_replay' entrypoint can now be used to record video.
* [gym/examples] Improve RLlib utilities to be more flexible. 
* [gym/examples] Fix logging performance issues of RLlib.
* [gym/examples] Improve documentation of RLlib tools.
* [gym/examples] Add monitoring of actually episode duration.

Co-authored-by: Alexis Duburcq <alexis.duburcq@wandercraft.eu>
  • Loading branch information
@duburcqa
duburcqa and Alexis Duburcq authored May 17, 2021
1 parent c7f3b95 commit f360ad0
Show file tree
Hide file tree
Showing 34 changed files with 1,141 additions and 425 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.6.15)
set(BUILD_VERSION 1.6.16)

# Set compatibility
if(CMAKE_VERSION VERSION_GREATER "3.11.0")
Expand Down
16 changes: 8 additions & 8 deletions core/include/jiminy/core/control/ControllerFunctor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,10 +73,10 @@ namespace jiminy
/// \return Return code to determine whether the execution of the method was successful.
///
///////////////////////////////////////////////////////////////////////////////////////////////
hresult_t computeCommand(float64_t const & t,
vectorN_t const & q,
vectorN_t const & v,
vectorN_t & command) override;
virtual hresult_t computeCommand(float64_t const & t,
vectorN_t const & q,
vectorN_t const & v,
vectorN_t & command) override;

///////////////////////////////////////////////////////////////////////////////////////////////
///
Expand All @@ -91,10 +91,10 @@ namespace jiminy
/// \return Return code to determine whether the execution of the method was successful.
///
///////////////////////////////////////////////////////////////////////////////////////////////
hresult_t internalDynamics(float64_t const & t,
vectorN_t const & q,
vectorN_t const & v,
vectorN_t & uCustom) override;
virtual hresult_t internalDynamics(float64_t const & t,
vectorN_t const & q,
vectorN_t const & v,
vectorN_t & uCustom) override;

private:
// std::conditional_t enables to use both functors and lambdas
Expand Down
3 changes: 3 additions & 0 deletions core/include/jiminy/core/engine/EngineMultiRobot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,6 +172,7 @@ namespace jiminy
config["enableConfiguration"] = true;
config["enableVelocity"] = true;
config["enableAcceleration"] = true;
config["enableForceExternal"] = false;
config["enableCommand"] = true;
config["enableMotorEffort"] = true;
config["enableEnergy"] = true;
Expand Down Expand Up @@ -284,6 +285,7 @@ namespace jiminy
bool_t const enableConfiguration;
bool_t const enableVelocity;
bool_t const enableAcceleration;
bool_t const enableForceExternal;
bool_t const enableCommand;
bool_t const enableMotorEffort;
bool_t const enableEnergy;
Expand All @@ -293,6 +295,7 @@ namespace jiminy
enableConfiguration(boost::get<bool_t>(options.at("enableConfiguration"))),
enableVelocity(boost::get<bool_t>(options.at("enableVelocity"))),
enableAcceleration(boost::get<bool_t>(options.at("enableAcceleration"))),
enableForceExternal(boost::get<bool_t>(options.at("enableForceExternal"))),
enableCommand(boost::get<bool_t>(options.at("enableCommand"))),
enableMotorEffort(boost::get<bool_t>(options.at("enableMotorEffort"))),
enableEnergy(boost::get<bool_t>(options.at("enableEnergy"))),
Expand Down
1 change: 1 addition & 0 deletions core/include/jiminy/core/engine/System.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -161,6 +161,7 @@ namespace jiminy
std::vector<std::string> positionFieldnames;
std::vector<std::string> velocityFieldnames;
std::vector<std::string> accelerationFieldnames;
std::vector<std::string> forceExternalFieldnames;
std::vector<std::string> commandFieldnames;
std::vector<std::string> motorEffortFieldnames;
std::string energyFieldname;
Expand Down
14 changes: 8 additions & 6 deletions core/include/jiminy/core/robot/Model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -353,6 +353,7 @@ namespace jiminy
std::vector<std::string> const & getPositionFieldnames(void) const;
std::vector<std::string> const & getVelocityFieldnames(void) const;
std::vector<std::string> const & getAccelerationFieldnames(void) const;
std::vector<std::string> const & getForceExternalFieldnames(void) const;

hresult_t getFlexibleConfigurationFromRigid(vectorN_t const & qRigid,
vectorN_t & qFlex) const;
Expand Down Expand Up @@ -427,16 +428,17 @@ namespace jiminy
vectorN_t constraintsDrift_; ///< Vector holding the drift of the constraints

vectorN_t positionLimitMin_; ///< Upper position limit of the whole configuration vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any)
vectorN_t positionLimitMax_; ///< Lower position limit of the whole configuration vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any)
vectorN_t velocityLimit_; ///< Maximum absolute velocity of the whole velocity vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any)
vectorN_t positionLimitMax_; ///< Lower position limit of the whole configuration vector
vectorN_t velocityLimit_; ///< Maximum absolute velocity of the whole velocity vector

std::vector<std::string> positionFieldnames_; ///< Fieldnames of the elements in the configuration vector of the rigid robot
std::vector<std::string> velocityFieldnames_; ///< Fieldnames of the elements in the velocity vector of the rigid robot
std::vector<std::string> accelerationFieldnames_; ///< Fieldnames of the elements in the acceleration vector of the rigid robot
std::vector<std::string> positionFieldnames_; ///< Fieldnames of the elements in the configuration vector of the model
std::vector<std::string> velocityFieldnames_; ///< Fieldnames of the elements in the velocity vector of the model
std::vector<std::string> accelerationFieldnames_; ///< Fieldnames of the elements in the acceleration vector of the model
std::vector<std::string> forceExternalFieldnames_; ///< Concatenated fieldnames of the external force applied at each joint of the model, 'universe' excluded

private:
pinocchio::Model pncModelFlexibleOrig_;
motionVector_t jointsAcceleration_; ///< Vector of joints acceleration corresponding to a copy of data.a - temporary buffer for computing constraints.
motionVector_t jointsAcceleration_; ///< Vector of joints acceleration corresponding to a copy of data.a - temporary buffer for computing constraints.

int32_t nq_;
int32_t nv_;
Expand Down
68 changes: 56 additions & 12 deletions core/src/engine/EngineMultiRobot.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -665,6 +665,9 @@ namespace jiminy
systemDataIt->accelerationFieldnames =
addCircumfix(systemIt->robot->getAccelerationFieldnames(),
systemIt->name, "", TELEMETRY_FIELDNAME_DELIMITER);
systemDataIt->forceExternalFieldnames =
addCircumfix(systemIt->robot->getForceExternalFieldnames(),
systemIt->name, "", TELEMETRY_FIELDNAME_DELIMITER);
systemDataIt->commandFieldnames =
addCircumfix(systemIt->robot->getCommandFieldnames(),
systemIt->name, "", TELEMETRY_FIELDNAME_DELIMITER);
Expand Down Expand Up @@ -703,6 +706,19 @@ namespace jiminy
systemDataIt->state.a);
}
}
if (engineOptions_->telemetry.enableForceExternal)
{
for (std::size_t i = 1; i < systemDataIt->state.fExternal.size(); ++i)
{
auto const & fext = systemDataIt->state.fExternal[i].toVector();
for (uint8_t j = 0; j < 6U; ++j)
{
returnCode = telemetrySender_.registerVariable(
systemDataIt->forceExternalFieldnames[(i - 1) * 6U + j],
fext[j]);
}
}
}
if (returnCode == hresult_t::SUCCESS)
{
if (engineOptions_->telemetry.enableCommand)
Expand Down Expand Up @@ -767,7 +783,7 @@ namespace jiminy
systemIt->robot->pncModel_,
systemIt->robot->pncData_);

// Update the telemetry internal state
// Update telemetry values
if (engineOptions_->telemetry.enableConfiguration)
{
telemetrySender_.updateValue(systemDataIt->positionFieldnames,
Expand All @@ -783,6 +799,19 @@ namespace jiminy
telemetrySender_.updateValue(systemDataIt->accelerationFieldnames,
systemDataIt->state.a);
}
if (engineOptions_->telemetry.enableForceExternal)
{
for (std::size_t i = 1; i < systemDataIt->state.fExternal.size(); ++i)
{
auto const & fext = systemDataIt->state.fExternal[i].toVector();
for (uint8_t j = 0; j < 6U; ++j)
{
telemetrySender_.updateValue(
systemDataIt->forceExternalFieldnames[(i - 1) * 6U + j],
fext[j]);
}
}
}
if (engineOptions_->telemetry.enableCommand)
{
telemetrySender_.updateValue(systemDataIt->commandFieldnames,
Expand Down Expand Up @@ -857,6 +886,14 @@ namespace jiminy
void computeExtraTerms(systemHolder_t & system,
systemDataHolder_t const & systemData)
{
/// This method is optimized to avoid redundant computations.
/// See `pinocchio::computeAllTerms` for reference:
///
/// Based on https://github.com/stack-of-tasks/pinocchio/blob/a1df23c2f183d84febdc2099e5fbfdbd1fc8018b/src/algorithm/compute-all-terms.hxx
///
/// Copyright (c) 2014-2020, CNRS
/// Copyright (c) 2018-2020, INRIA

pinocchio::Model const & model = system.robot->pncModel_;
pinocchio::Data & data = system.robot->pncData_;

Expand Down Expand Up @@ -887,16 +924,6 @@ namespace jiminy
}
}

// Now that Ycrb is available, it is possible to infer directly the subtree center of masses
pinocchio::getComFromCrba(model, data);
data.Ig.mass() = data.oYcrb[0].mass();
data.Ig.lever().setZero();
data.Ig.inertia() = data.oYcrb[0].inertia();
for (int32_t i = 1; i < model.njoints; ++i)
{
data.com[i] = data.oMi[i].actInv(data.oYcrb[i].lever());
}

/* Neither 'aba' nor 'forwardDynamics' are computed the actual joints
acceleration and forces, so it must be done separately:
- 1st step: computing the forces based on rnea algorithm
Expand Down Expand Up @@ -928,7 +955,19 @@ namespace jiminy
}
}

pinocchio_overload::forwardKinematicsAcceleration(model, data, data.ddq);
/* Now that `data.oYcrb` and `data.h` are available, one can get directly
the position and velocity of the center of mass of each subtrees. */
data.Ig.mass() = data.oYcrb[0].mass();
data.Ig.lever().setZero();
data.Ig.inertia() = data.oYcrb[0].inertia();
data.com[0] = data.oYcrb[0].lever();
for (int32_t i = 1; i < model.njoints; ++i)
{
data.com[i] = data.oMi[i].actInv(data.oYcrb[i].lever());
data.vcom[i] = data.h[i].linear() / data.mass[i];
}

pinocchio_overload::forwardKinematicsAcceleration(model, data, data.ddq);
}

void computeAllExtraTerms(std::vector<systemHolder_t> & systems,
Expand Down Expand Up @@ -2216,6 +2255,11 @@ namespace jiminy
template<typename ...Args>
std::tuple<bool_t, float64_t> isGcdIncluded(std::vector<systemDataHolder_t> const & systemsDataHolder, Args... values)
{
if (systemsDataHolder.empty())
{
return isGcdIncluded(std::forward<Args>(values)...);
}

float64_t minValue = INF;
auto lambda = [&minValue, &values...](systemDataHolder_t const & systemData)
{
Expand Down
1 change: 1 addition & 0 deletions core/src/engine/System.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -182,6 +182,7 @@ namespace jiminy
positionFieldnames(),
velocityFieldnames(),
accelerationFieldnames(),
forceExternalFieldnames(),
commandFieldnames(),
motorEffortFieldnames(),
energyFieldname(),
Expand Down
Loading

0 comments on commit f360ad0

Please sign in to comment.