Skip to content

Commit

Permalink
[core] Remove return code in favor of throwing exceptions.
Browse files Browse the repository at this point in the history
  • Loading branch information
duburcqa committed Feb 11, 2024
1 parent c904739 commit 39a1a24
Show file tree
Hide file tree
Showing 89 changed files with 3,748 additions and 5,583 deletions.
5 changes: 3 additions & 2 deletions .github/workflows/macos.yml
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,8 @@ jobs:
strategy:
matrix:
OS: ['macos-14'] # 'macos-13': Intel (x86), 'macos-14': Apple Silicon (arm64)
OS: ['macos-13'] # 'macos-13': Intel (x86), 'macos-14': Apple Silicon (arm64)
# meshcat video recorder 'playwright' is not working on 'macos-14' runners.
PYTHON_VERSION: ['3.10', '3.11', '3.12'] # `setup-python` does not support Python<3.10 on Apple Silicon
BUILD_TYPE: ['Release']
include:
Expand All @@ -25,7 +26,7 @@ jobs:
- OS: 'macos-13'
PYTHON_VERSION: '3.9'
BUILD_TYPE: 'Release'
- OS: 'macos-14'
- OS: 'macos-13'
PYTHON_VERSION: '3.11'
BUILD_TYPE: 'Debug'

Expand Down
15 changes: 14 additions & 1 deletion build_tools/cmake/base.cmake
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,18 @@ if(CMAKE_VERSION VERSION_GREATER_EQUAL 3.27.0)
cmake_policy(SET CMP0144 NEW)
endif()

# Forces GCC/Clang compilers to enable color diagnostics.
# CMake versions 3.24 and below do not support this option, so we have
# to invoke the color diagnostics flags manually.
set(CMAKE_COLOR_DIAGNOSTICS ON)
if(CMAKE_VERSION VERSION_LESS "3.24.0")
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
add_compile_options(-fdiagnostics-color=always)
elseif(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
add_compile_options(-fcolor-diagnostics)
endif()
endif()

# Check if network is available before compiling external projects
if(WIN32)
find_program(HAS_PING "ping")
Expand Down Expand Up @@ -54,7 +66,8 @@ else()
-Wno-unknown-warning -Wno-undefined-var-template \
-Wno-long-long -Wno-error=maybe-uninitialized \
-Wno-error=uninitialized -Wno-error=deprecated \
-Wno-error=array-bounds -Wno-error=redundant-move")
-Wno-error=array-bounds -Wno-error=redundant-move \
-Wno-error=suggest-attribute=noreturn")
endif()

# Shared libraries need PIC
Expand Down
21 changes: 10 additions & 11 deletions core/examples/double_pendulum/double_pendulum.cc
Original file line number Diff line number Diff line change
Expand Up @@ -85,8 +85,8 @@ int main(int /* argc */, char * /* argv */[])
controller->initialize(robot);

// Instantiate and configuration the engine
auto engine = std::make_shared<Engine>();
GenericConfig simuOptions = engine->getOptions();
Engine engine{};
GenericConfig simuOptions = engine.getOptions();
GenericConfig & telemetryOptions = boost::get<GenericConfig>(simuOptions.at("telemetry"));
boost::get<bool>(telemetryOptions.at("isPersistent")) = true;
boost::get<bool>(telemetryOptions.at("enableConfiguration")) = true;
Expand Down Expand Up @@ -118,8 +118,8 @@ int main(int /* argc */, char * /* argv */[])
boost::get<double>(contactsOptions.at("friction")) = 5.0;
boost::get<double>(contactsOptions.at("transitionEps")) = 0.001;
boost::get<double>(contactsOptions.at("transitionVelocity")) = 0.01;
engine->setOptions(simuOptions);
engine->initialize(robot, controller, callback);
engine.setOptions(simuOptions);
engine.initialize(robot, controller, callback);
std::cout << "Initialization: " << timer.toc<std::milli>() << "ms" << std::endl;

// =====================================================================
Expand All @@ -134,20 +134,19 @@ int main(int /* argc */, char * /* argv */[])

// Run simulation
timer.tic();
engine->simulate(tf, q0, v0);
engine.simulate(tf, q0, v0);
std::cout << "Simulation: " << timer.toc<std::milli>() << "ms" << std::endl;

// Write the log file
std::vector<std::string> fieldnames;
std::shared_ptr<const LogData> logData;
engine->getLog(logData);
std::cout << logData->times.size() << " log points" << std::endl;
std::cout << engine->getStepperState().iter << " internal integration steps" << std::endl;
std::shared_ptr<const LogData> logDataPtr = engine.getLog();
std::cout << logDataPtr->times.size() << " log points" << std::endl;
std::cout << engine.getStepperState().iter << " internal integration steps" << std::endl;
timer.tic();
engine->writeLog((outputDirPath / "log.data").string(), "binary");
engine.writeLog((outputDirPath / "log.data").string(), "binary");
std::cout << "Write log binary: " << timer.toc<std::milli>() << "ms" << std::endl;
timer.tic();
engine->writeLog((outputDirPath / "log.hdf5").string(), "hdf5");
engine.writeLog((outputDirPath / "log.hdf5").string(), "hdf5");
std::cout << "Write log HDF5: " << timer.toc<std::milli>() << "ms" << std::endl;

return 0;
Expand Down
17 changes: 8 additions & 9 deletions core/examples/external_project/double_pendulum.cc
Original file line number Diff line number Diff line change
Expand Up @@ -81,8 +81,8 @@ int main(int argc, char * argv[])
controller->initialize(robot);

// Instantiate the engine
auto engine = std::make_shared<Engine>();
engine->initialize(robot, controller, callback);
Engine engine{};
engine.initialize(robot, controller, callback);
std::cout << "Initialization: " << timer.toc<std::milli>() << "ms" << std::endl;

// =====================================================================
Expand All @@ -97,19 +97,18 @@ int main(int argc, char * argv[])

// Run simulation
timer.tic();
engine->simulate(tf, q0, v0);
engine.simulate(tf, q0, v0);
std::cout << "Simulation: " << timer.toc<std::milli>() << "ms" << std::endl;

// Write the log file
std::vector<std::string> fieldnames;
std::shared_ptr<const LogData> logData;
engine->getLog(logData);
std::cout << logData->times.size() << " log points" << std::endl;
std::cout << engine->getStepperState().iter << " internal integration steps" << std::endl;
std::shared_ptr<const LogData> logDataPtr = engine.getLog();
std::cout << logDataPtr->times.size() << " log points" << std::endl;
std::cout << engine.getStepperState().iter << " internal integration steps" << std::endl;
timer.tic();
engine->writeLog((outputDirPath / "log.data").string(), "binary");
engine.writeLog((outputDirPath / "log.data").string(), "binary");
std::cout << "Write log binary: " << timer.toc<std::milli>() << "ms" << std::endl;
engine->writeLog((outputDirPath / "log.hdf5").string(), "hdf5");
engine.writeLog((outputDirPath / "log.hdf5").string(), "hdf5");
std::cout << "Write log HDF5: " << timer.toc<std::milli>() << "ms" << std::endl;

return 0;
Expand Down
14 changes: 7 additions & 7 deletions core/include/jiminy/core/constraints/abstract_constraint.h
Original file line number Diff line number Diff line change
Expand Up @@ -29,17 +29,17 @@ namespace jiminy
///
/// \remark This method is not intended to be called manually. The Robot to which the
/// constraint is added is taking care of it when its own `reset` method is called.
virtual hresult_t reset(const Eigen::VectorXd & q, const Eigen::VectorXd & v) = 0;
virtual void reset(const Eigen::VectorXd & q, const Eigen::VectorXd & v) = 0;

void enable();
void disable();
bool getIsEnabled() const;

hresult_t setBaumgartePositionGain(double kp);
void setBaumgartePositionGain(double kp);
double getBaumgartePositionGain() const;
hresult_t setBaumgarteVelocityGain(double kd);
void setBaumgarteVelocityGain(double kd);
double getBaumgarteVelocityGain() const;
hresult_t setBaumgarteFreq(double freq);
void setBaumgarteFreq(double freq);
/// \brief Natural frequency of critically damping position/velocity error correction.
double getBaumgarteFreq() const;

Expand All @@ -50,8 +50,8 @@ namespace jiminy
///
/// \param[in] q Current joint position.
/// \param[in] v Current joint velocity.
virtual hresult_t computeJacobianAndDrift(const Eigen::VectorXd & q,
const Eigen::VectorXd & v) = 0;
virtual void computeJacobianAndDrift(const Eigen::VectorXd & q,
const Eigen::VectorXd & v) = 0;

virtual const std::string & getType() const = 0;

Expand All @@ -70,7 +70,7 @@ namespace jiminy
/// \param[in] model Model on which to apply the constraint.
///
/// \return Error code: attach may fail, including if the constraint is already attached.
hresult_t attach(std::weak_ptr<const Model> model);
void attach(std::weak_ptr<const Model> model);

/// \brief Detach the constraint from its model.
void detach();
Expand Down
9 changes: 4 additions & 5 deletions core/include/jiminy/core/constraints/distance_constraint.h
Original file line number Diff line number Diff line change
Expand Up @@ -27,14 +27,13 @@ namespace jiminy
const std::array<std::string, 2> & getFramesNames() const noexcept;
const std::array<pinocchio::FrameIndex, 2> & getFrameIndices() const noexcept;

hresult_t setReferenceDistance(double distanceRef);
void setReferenceDistance(double distanceRef);
double getReferenceDistance() const noexcept;

virtual hresult_t reset(const Eigen::VectorXd & q,
const Eigen::VectorXd & v) override final;
virtual void reset(const Eigen::VectorXd & q, const Eigen::VectorXd & v) override final;

virtual hresult_t computeJacobianAndDrift(const Eigen::VectorXd & q,
const Eigen::VectorXd & v) override final;
virtual void computeJacobianAndDrift(const Eigen::VectorXd & q,
const Eigen::VectorXd & v) override final;

private:
/// \brief Names of the frames on which the constraint operates.
Expand Down
7 changes: 3 additions & 4 deletions core/include/jiminy/core/constraints/frame_constraint.h
Original file line number Diff line number Diff line change
Expand Up @@ -39,11 +39,10 @@ namespace jiminy
void setNormal(const Eigen::Vector3d & normal) noexcept;
const Eigen::Matrix3d & getLocalFrame() const noexcept;

virtual hresult_t reset(const Eigen::VectorXd & q,
const Eigen::VectorXd & v) override final;
virtual void reset(const Eigen::VectorXd & q, const Eigen::VectorXd & v) override final;

virtual hresult_t computeJacobianAndDrift(const Eigen::VectorXd & q,
const Eigen::VectorXd & v) override final;
virtual void computeJacobianAndDrift(const Eigen::VectorXd & q,
const Eigen::VectorXd & v) override final;

private:
/// \brief Name of the frame on which the constraint operates.
Expand Down
7 changes: 3 additions & 4 deletions core/include/jiminy/core/constraints/joint_constraint.h
Original file line number Diff line number Diff line change
Expand Up @@ -33,11 +33,10 @@ namespace jiminy
void setRotationDir(bool isReversed) noexcept;
bool getRotationDir() noexcept;

virtual hresult_t reset(const Eigen::VectorXd & q,
const Eigen::VectorXd & v) override final;
virtual void reset(const Eigen::VectorXd & q, const Eigen::VectorXd & v) override final;

virtual hresult_t computeJacobianAndDrift(const Eigen::VectorXd & q,
const Eigen::VectorXd & v) override final;
virtual void computeJacobianAndDrift(const Eigen::VectorXd & q,
const Eigen::VectorXd & v) override final;

private:
/// \brief Name of the joint on which the constraint operates.
Expand Down
8 changes: 4 additions & 4 deletions core/include/jiminy/core/constraints/sphere_constraint.h
Original file line number Diff line number Diff line change
Expand Up @@ -38,11 +38,11 @@ namespace jiminy
void setReferenceTransform(const pinocchio::SE3 & transformRef) noexcept;
const pinocchio::SE3 & getReferenceTransform() const noexcept;

virtual hresult_t reset(const Eigen::VectorXd & /* q */,
const Eigen::VectorXd & /* v */) override final;
virtual void reset(const Eigen::VectorXd & /* q */,
const Eigen::VectorXd & /* v */) override final;

virtual hresult_t computeJacobianAndDrift(const Eigen::VectorXd & q,
const Eigen::VectorXd & v) override final;
virtual void computeJacobianAndDrift(const Eigen::VectorXd & q,
const Eigen::VectorXd & v) override final;

private:
/// \brief Name of the frame on which the constraint operates.
Expand Down
8 changes: 4 additions & 4 deletions core/include/jiminy/core/constraints/wheel_constraint.h
Original file line number Diff line number Diff line change
Expand Up @@ -40,11 +40,11 @@ namespace jiminy
void setReferenceTransform(const pinocchio::SE3 & transformRef) noexcept;
const pinocchio::SE3 & getReferenceTransform() const noexcept;

virtual hresult_t reset(const Eigen::VectorXd & /* q */,
const Eigen::VectorXd & /* v */) override final;
virtual void reset(const Eigen::VectorXd & /* q */,
const Eigen::VectorXd & /* v */) override final;

virtual hresult_t computeJacobianAndDrift(const Eigen::VectorXd & q,
const Eigen::VectorXd & v) override final;
virtual void computeJacobianAndDrift(const Eigen::VectorXd & q,
const Eigen::VectorXd & v) override final;

private:
/// \brief Name of the frame on which the constraint operates.
Expand Down
50 changes: 17 additions & 33 deletions core/include/jiminy/core/control/abstract_controller.h
Original file line number Diff line number Diff line change
Expand Up @@ -56,9 +56,7 @@ namespace jiminy
/// \brief Set the parameters of the controller.
///
/// \param[in] robot Robot
///
/// \return Return code to determine whether the execution of the method was successful.
virtual hresult_t initialize(std::weak_ptr<const Robot> robot);
virtual void initialize(std::weak_ptr<const Robot> robot);

/// \brief Dynamically registered a scalar variable to the telemetry. It is the main entry
/// point for a user to log custom variables.
Expand All @@ -70,24 +68,20 @@ namespace jiminy
///
/// \param[in] name Name of the variable. It will appear in the header of the log.
/// \param[in] values Variable to add to the telemetry.
///
/// \return Return code to determine whether the execution of the method was successful.
template<typename T>
hresult_t registerVariable(const std::string_view & name, const T & value);
void registerVariable(const std::string_view & name, const T & value);

/// \brief Dynamically registered a Eigen Vector to the telemetry.
///
/// \param[in] fieldnames Name of each element of the variable. It will appear in the
/// header of the log.
/// \param[in] values Eigen vector to add to the telemetry. It accepts non-contiguous
/// temporary.
///
/// \return Return code to determine whether the execution of the method was successful.
hresult_t registerVariable(
void registerVariable(
const std::vector<std::string> & fieldnames,
const Eigen::Ref<VectorX<double>, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>> &
values);
hresult_t registerVariable(
void registerVariable(
const std::vector<std::string> & fieldnames,
const Eigen::Ref<VectorX<int64_t>, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>> &
values);
Expand All @@ -96,10 +90,8 @@ namespace jiminy
///
/// \param[in] name Name of the variable.
/// \param[in] values Variable to add to the telemetry
///
/// \return Return code to determine whether the execution of the method was successful.
template<typename T>
hresult_t registerConstant(const std::string_view & name, const T & value);
void registerConstant(const std::string_view & name, const T & value);

/// \brief Remove all variables dynamically registered to the telemetry.
///
Expand All @@ -115,12 +107,10 @@ namespace jiminy
/// \param[in] q Current configuration vector.
/// \param[in] v Current velocity vector.
/// \param[out] command Output effort vector.
///
/// \return Return code to determine whether the execution of the method was successful.
virtual hresult_t computeCommand(double t,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
Eigen::VectorXd & command) = 0;
virtual void computeCommand(double t,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
Eigen::VectorXd & command) = 0;

/// \brief Emulate custom phenomenon that are part of the internal dynamics of the system
/// but not included in the physics engine.
Expand All @@ -129,12 +119,10 @@ namespace jiminy
/// \param[in] q Current configuration vector.
/// \param[in] v Current velocity vector.
/// \param[in] uCustom Output effort vector.
///
/// \return Return code to determine whether the execution of the method was successful.
virtual hresult_t internalDynamics(double t,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
Eigen::VectorXd & uCustom) = 0;
virtual void internalDynamics(double t,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
Eigen::VectorXd & uCustom) = 0;

/// \brief Dictionary with the parameters of the controller.
GenericConfig getOptions() const noexcept;
Expand All @@ -144,9 +132,7 @@ namespace jiminy
/// \details Note that one must reset Jiminy Engine for this to take effect.
///
/// \param[in] controllerOptions Dictionary with the parameters of the controller.
///
/// \return Return code to determine whether the execution of the method was successful.
hresult_t setOptions(const GenericConfig & controllerOptions);
void setOptions(const GenericConfig & controllerOptions);

/// \brief Configure the telemetry of the controller.
///
Expand All @@ -160,10 +146,8 @@ namespace jiminy
/// it before flushing the telemetry data at the end of each simulation steps.
///
/// \param[in] telemetryData Shared pointer to the robot-wide telemetry data object
///
/// \return Return code to determine whether the execution of the method was successful.
virtual hresult_t configureTelemetry(std::shared_ptr<TelemetryData> telemetryData,
const std::string & prefix = {});
virtual void configureTelemetry(std::shared_ptr<TelemetryData> telemetryData,
const std::string & prefix = {});

/// \brief Update the internal buffers of the telemetry associated with variables monitored
/// by the controller.
Expand All @@ -188,7 +172,7 @@ namespace jiminy
///
/// \param[in] resetDynamicTelemetry Whether variables dynamically registered to the
/// telemetry must be removed. Optional: False by default.
virtual hresult_t reset(bool resetDynamicTelemetry = false);
virtual void reset(bool resetDynamicTelemetry = false);

/// \brief Whether the controller has been initialized.
///
Expand Down
Loading

0 comments on commit 39a1a24

Please sign in to comment.