Skip to content

Commit

Permalink
[Gym] Generic locomotion envs - PART III (#197)
Browse files Browse the repository at this point in the history 
* [core] Add new 'successiveIterFailedMax' stepper option.
* [core] Fix collision body contact force computation and primitive geometry support. Fix add frame. Fix generic robot envs.
* [core] Improve mesh package support. Add meshPackageDirs optional argument to model.initialize method. 
* [core] Expose sensor data individually. Add sensor __repr__. Expose EngineMultiRobot systems vector in Python. Fix systemData returning unusable robot and controller.
* [engine_async] Improve viewer backend management.
* [unit_py] Rework the unit test in Python to reduce code duplication. Add dedicated box and sphere testing urdf with collision and visual.
* [misc] Reduce static library compile size by removing debugging symbols.

* [python/robot] Set controller and sensors update periods.

Co-authored-by: Alexis Duburcq <alexis.duburcq@wandercraft.eu>
  • Loading branch information
@duburcqa
duburcqa and Alexis Duburcq authored Sep 16, 2020
1 parent b3c34a5 commit c359d9f
Show file tree
Hide file tree
Showing 26 changed files with 1,499 additions and 1,035 deletions.
3 changes: 2 additions & 1 deletion .github/workflows/manylinux.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,11 +65,12 @@ jobs:
-DBoost_NO_SYSTEM_PATHS=TRUE -DBoost_NO_BOOST_CMAKE=TRUE \
-DBoost_USE_STATIC_LIBS=OFF -DPYTHON_EXECUTABLE="${PYTHON_EXECUTABLE}" \
-DBUILD_TESTING=ON -DBUILD_EXAMPLES=ON -DBUILD_PYTHON_INTERFACE=ON \
-DCMAKE_CXX_FLAGS="-fPIC" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
-DCMAKE_CXX_FLAGS="-fPIC -s" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
make -j2
mkdir -p "$RootDir/build/pypi/jiminy_py/src/jiminy_py"
cp -r "$InstallDir/lib/python${{ matrix.PYTHON_VERSION }}/site-packages/." "$RootDir/build/pypi/jiminy_py/src/jiminy_py"
find "$RootDir/build/pypi/" -name "*.so*" -exec strip --strip-unneeded -v {} +
make install
Expand Down
18 changes: 9 additions & 9 deletions build_tools/build_install_deps_linux.sh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,7 +114,7 @@ mkdir -p "$RootDir/boost/build"
--with-thread --with-serialization --with-test --with-python \
--build-type=minimal architecture=x86 address-model=64 threading=multi \
--layout=system link=shared runtime-link=shared \
toolset=gcc cxxflags="-std=c++11 -fPIC" variant="$BuildTypeB2" install -q -d0 -j2
toolset=gcc cxxflags="-std=c++11 -fPIC -s" variant="$BuildTypeB2" install -q -d0 -j2

#################################### Build and install eigen3 ##########################################

Expand All @@ -134,23 +134,23 @@ cmake "$RootDir/eigenpy" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$Instal
-DPYTHON_STANDARD_LAYOUT=ON -DBoost_NO_SYSTEM_PATHS=TRUE -DBoost_NO_BOOST_CMAKE=TRUE \
-DBOOST_ROOT="$InstallDir" -DBoost_INCLUDE_DIR="$InstallDir/include" -DBoost_USE_STATIC_LIBS=OFF \
-DBUILD_TESTING=OFF -DINSTALL_DOCUMENTATION=OFF \
-DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
-DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC -s" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
make install -j2

################################## Build and install tinyxml ###########################################

mkdir -p "$RootDir/tinyxml/build"
cd "$RootDir/tinyxml/build"
cmake "$RootDir/tinyxml" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" \
-DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC -DTIXML_USE_STL" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
-DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC -s -DTIXML_USE_STL" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
make install -j2

############################## Build and install console_bridge ########################################

mkdir -p "$RootDir/console_bridge/build"
cd "$RootDir/console_bridge/build"
cmake "$RootDir/console_bridge" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" \
-DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
-DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC -s" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
make install -j2

############################### Build and install urdfdom_headers ######################################
Expand All @@ -167,7 +167,7 @@ mkdir -p "$RootDir/urdfdom/build"
cd "$RootDir/urdfdom/build"
cmake "$RootDir/urdfdom" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" \
-DCMAKE_PREFIX_PATH="$InstallDir" -DBUILD_TESTING=OFF \
-DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
-DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC -s" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
make install -j2

###################################### Build and install assimp ########################################
Expand All @@ -177,15 +177,15 @@ cd "$RootDir/assimp/build"
cmake "$RootDir/assimp" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" \
-DASSIMP_BUILD_ASSIMP_TOOLS=OFF -DASSIMP_BUILD_ZLIB=ON -DASSIMP_BUILD_TESTS=OFF \
-DASSIMP_BUILD_SAMPLES=OFF -DBUILD_DOCS=OFF \
-DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC -Wno-strict-overflow -Wno-class-memaccess" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
-DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC -s -Wno-strict-overflow -Wno-class-memaccess" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
make install -j2

############################# Build and install qhull and hpp-fcl ######################################

mkdir -p "$RootDir/hpp-fcl/third-parties/qhull/build"
cd "$RootDir/hpp-fcl/third-parties/qhull/build"
cmake "$RootDir/hpp-fcl/third-parties/qhull" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" \
-DBUILD_SHARED_LIBS=OFF -DBUILD_STATIC_LIBS=ON -DCMAKE_CXX_FLAGS="-fPIC" -DCMAKE_C_FLAGS="-fPIC" \
-DBUILD_SHARED_LIBS=OFF -DBUILD_STATIC_LIBS=ON -DCMAKE_CXX_FLAGS="-fPIC -s" -DCMAKE_C_FLAGS="-fPIC -s" \
-DCMAKE_BUILD_TYPE="$BUILD_TYPE"
make install -j2

Expand All @@ -196,7 +196,7 @@ cmake "$RootDir/hpp-fcl" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$Instal
-DPYTHON_STANDARD_LAYOUT=ON -DBoost_NO_SYSTEM_PATHS=TRUE -DBoost_NO_BOOST_CMAKE=TRUE \
-DBOOST_ROOT="$InstallDir" -DBoost_INCLUDE_DIR="$InstallDir/include" -DBoost_USE_STATIC_LIBS=OFF \
-DBUILD_PYTHON_INTERFACE=ON -DHPP_FCL_HAS_QHULL=ON -DINSTALL_DOCUMENTATION=OFF \
-DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC -Wno-unused-parameter" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
-DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC -s -Wno-unused-parameter" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
make install -j2

################################# Build and install Pinocchio ##########################################
Expand All @@ -210,5 +210,5 @@ cmake "$RootDir/pinocchio" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$Inst
-DBOOST_ROOT="$InstallDir" -DBoost_INCLUDE_DIR="$InstallDir/include" -DBoost_USE_STATIC_LIBS=OFF \
-DBUILD_WITH_COLLISION_SUPPORT=ON -DBUILD_TESTING=OFF -DINSTALL_DOCUMENTATION=OFF \
-DBUILD_WITH_URDF_SUPPORT=ON -DBUILD_PYTHON_INTERFACE=ON \
-DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC -Wno-unused-local-typedefs -Wno-uninitialized" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
-DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC -s -Wno-unused-local-typedefs -Wno-uninitialized" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
make install -j2
12 changes: 8 additions & 4 deletions core/include/jiminy/core/engine/EngineMultiRobot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -312,6 +312,7 @@ namespace jiminy
config["tolRel"] = 1.0e-4;
config["dtMax"] = SIMULATION_MAX_TIMESTEP;
config["dtRestoreThresholdRel"] = 0.2;
config["successiveIterFailedMax"] = 1000U;
config["iterMax"] = -1; // <= 0: disable
config["timeout"] = 0.0; // <= 0.0: disable
config["sensorsUpdatePeriod"] = 0.0;
Expand Down Expand Up @@ -407,6 +408,7 @@ namespace jiminy
float64_t const tolRel;
float64_t const dtMax;
float64_t const dtRestoreThresholdRel;
uint32_t const successiveIterFailedMax;
int32_t const iterMax;
float64_t const timeout;
float64_t const sensorsUpdatePeriod;
Expand All @@ -421,6 +423,7 @@ namespace jiminy
tolRel(boost::get<float64_t>(options.at("tolRel"))),
dtMax(boost::get<float64_t>(options.at("dtMax"))),
dtRestoreThresholdRel(boost::get<float64_t>(options.at("dtRestoreThresholdRel"))),
successiveIterFailedMax(boost::get<uint32_t>(options.at("successiveIterFailedMax"))),
iterMax(boost::get<int32_t>(options.at("iterMax"))),
timeout(boost::get<float64_t>(options.at("timeout"))),
sensorsUpdatePeriod(boost::get<float64_t>(options.at("sensorsUpdatePeriod"))),
Expand Down Expand Up @@ -576,6 +579,7 @@ namespace jiminy
configHolder_t getOptions(void) const;
hresult_t setOptions(configHolder_t const & engineOptions);
bool_t getIsTelemetryConfigured(void) const;
std::vector<std::string> getSystemsNames(void) const;
hresult_t getSystem(std::string const & systemName,
systemDataHolder_t const * & system) const;
hresult_t getSystem(std::string const & systemName,
Expand Down Expand Up @@ -629,23 +633,23 @@ namespace jiminy
///
/// \param[in] system System for which to perform computation.
/// \param[in] collisionPairIdx Id of the collision pair associated with the body
/// \return Contact force, at parent frame, in the global frame.
/// \return Contact force, at parent joint, in the local frame.
pinocchio::Force computeContactDynamicsAtBody(systemDataHolder_t const & system,
int32_t const & collisionPairIdx) const;

/// \brief Compute the force resulting from ground contact on a given frame.
///
/// \param[in] system System for which to perform computation.
/// \param[in] frameIdx Id of the frame in contact.
/// \return Contact force, in the global frame.
/// \return Contact force, at parent joint, in the local frame.
pinocchio::Force computeContactDynamicsAtFrame(systemDataHolder_t const & system,
int32_t const & frameIdx) const;

/// \brief Compute the force resulting from ground contact for a given normal direction and depth.
pinocchio::Force computeContactDynamics(systemDataHolder_t const & system,
vector3_t const & nGround,
float64_t const & depth,
vector3_t const & vFrameInWorld) const;
vector3_t const & vContactInWorld) const;

void computeCommand(systemDataHolder_t & system,
float64_t const & t,
Expand Down Expand Up @@ -723,12 +727,12 @@ namespace jiminy

public:
std::unique_ptr<engineOptions_t const> engineOptions_;
std::vector<systemDataHolder_t> systemsDataHolder_;

protected:
bool_t isTelemetryConfigured_;
bool_t isSimulationRunning_;
configHolder_t engineOptionsHolder_;
std::vector<systemDataHolder_t> systemsDataHolder_;

private:
TelemetrySender telemetrySender_;
Expand Down
3 changes: 1 addition & 2 deletions core/include/jiminy/core/robot/BasicSensors.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,6 @@ namespace jiminy

std::string const & getFrameName(void) const;
int32_t const & getFrameIdx(void) const;
std::string const & getBodyName(void) const;
int32_t getJointIdx(void) const;

private:
Expand All @@ -94,7 +93,7 @@ namespace jiminy
private:
std::string frameName_;
int32_t frameIdx_;
int32_t parentBodyFrameIdx_;
int32_t parentJointIdx_;
};

class EncoderSensor : public AbstractSensorTpl<EncoderSensor>
Expand Down
19 changes: 14 additions & 5 deletions core/include/jiminy/core/robot/Model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -139,9 +139,15 @@ namespace jiminy
Model(void);
virtual ~Model(void) = default;

hresult_t initialize(std::string const & urdfPath,
bool_t const & hasFreeflyer = true);

hresult_t initialize(std::string const & urdfPath,
bool_t const & hasFreeflyer = true,
std::vector<std::string> const & meshPackageDirs = {});

/// \brief Add a frame in the kinematic tree, attached to the frame of an existing body.
///
/// \param[in] frameName Name of the frame to be added
/// \param[in] parentBodyName Name of the parent body frame
/// \param[in] framePlacement Frame placement wrt the parent body frame
hresult_t addFrame(std::string const & frameName,
std::string const & parentBodyName,
pinocchio::SE3 const & framePlacement);
Expand All @@ -159,6 +165,7 @@ namespace jiminy

bool_t const & getIsInitialized(void) const;
std::string const & getUrdfPath(void) const;
std::vector<std::string> const & getMeshPackageDirs(void) const;
bool_t const & getHasFreeflyer(void) const;
// Getters without 'get' prefix for consistency with pinocchio C++ API
int32_t const & nq(void) const;
Expand Down Expand Up @@ -190,8 +197,9 @@ namespace jiminy
vectorN_t & xRigid) const;

protected:
hresult_t loadUrdfModel(std::string const & urdfPath,
bool_t const & hasFreeflyer);
hresult_t loadUrdfModel(std::string const & urdfPath,
bool_t const & hasFreeflyer,
std::vector<std::string> meshPackageDirs);
hresult_t generateModelFlexible(void);
hresult_t generateModelBiased(void);
hresult_t refreshCollisionsProxies(void);
Expand All @@ -211,6 +219,7 @@ namespace jiminy
protected:
bool_t isInitialized_;
std::string urdfPath_;
std::vector<std::string> meshPackageDirs_;
bool_t hasFreeflyer_;
configHolder_t mdlOptionsHolder_;

Expand Down
5 changes: 3 additions & 2 deletions core/include/jiminy/core/robot/Robot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,8 +47,9 @@ namespace jiminy
Robot(void);
virtual ~Robot(void);

hresult_t initialize(std::string const & urdfPath,
bool_t const & hasFreeflyer = true);
hresult_t initialize(std::string const & urdfPath,
bool_t const & hasFreeflyer = true,
std::vector<std::string> const & meshPackageDirs = {});

hresult_t attachMotor(std::shared_ptr<AbstractMotorBase> motor);
hresult_t getMotor(std::string const & motorName,
Expand Down
Loading

0 comments on commit c359d9f

Please sign in to comment.