diff --git a/CMakeLists.txt b/CMakeLists.txt index b7d845a60..66f6993de 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.12.4) # Set the build version (specify a tweak version to indicated post-release if needed) -set(BUILD_VERSION 1.8.1) +set(BUILD_VERSION 1.8.2) # Set compatibility set(COMPATIBILITY_VERSION SameMinorVersion) diff --git a/INSTALL.md b/INSTALL.md index 1a1e198f4..e8ce76292 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -79,7 +79,7 @@ make install -j2 ```bash sudo apt install -y gnupg curl wget build-essential cmake doxygen graphviz -python -m pip install "numpy>=1.21,<2.0" +python -m pip install "wheel", "numpy>=1.21,<2.0" ``` ### Jiminy dependencies build and install diff --git a/build_tools/build_install_deps_unix.sh b/build_tools/build_install_deps_unix.sh index 9cccd42cd..e52ea7fc1 100755 --- a/build_tools/build_install_deps_unix.sh +++ b/build_tools/build_install_deps_unix.sh @@ -23,18 +23,19 @@ fi ### Set the compiler(s) if undefined if [ -z ${CMAKE_C_COMPILER} ]; then - CMAKE_C_COMPILER="gcc" + CMAKE_C_COMPILER="$(which gcc)" echo "CMAKE_C_COMPILER is unset. Defaulting to '${CMAKE_C_COMPILER}'." fi if [ -z ${CMAKE_CXX_COMPILER} ]; then - CMAKE_CXX_COMPILER="g++" + CMAKE_CXX_COMPILER="$(which g++)" echo "CMAKE_CXX_COMPILER is unset. Defaulting to '${CMAKE_CXX_COMPILER}'." fi ### Set common CMAKE_C/CXX_FLAGS # '_LIBCPP_ENABLE_CXX17_REMOVED_UNARY_BINARY_FUNCTION' flag is required to compile `boost::container_hash::hash` # with C++17 enabled from "old" releases of Boost. -CMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -D_LIBCPP_ENABLE_CXX17_REMOVED_UNARY_BINARY_FUNCTION -Wno-deprecated-declarations" +CMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -D_LIBCPP_ENABLE_CXX17_REMOVED_UNARY_BINARY_FUNCTION $( + ) -Wno-deprecated-declarations -Wno-enum-constexpr-conversion" if [ "${BUILD_TYPE}" == "Release" ]; then CMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -DNDEBUG -O3" elif [ "${BUILD_TYPE}" == "Debug" ]; then @@ -271,6 +272,7 @@ mkdir -p "${RootDir}/boost/build" mkdir -p "${RootDir}/eigen3/build" cd "${RootDir}/eigen3/build" cmake "${RootDir}/eigen3" -Wno-dev -DCMAKE_CXX_STANDARD=17 \ + -DCMAKE_C_COMPILER="${CMAKE_C_COMPILER}" -DCMAKE_CXX_COMPILER="${CMAKE_CXX_COMPILER}" \ -DCMAKE_INSTALL_PREFIX="${InstallDir}" \ -DBUILD_TESTING=OFF -DEIGEN_BUILD_PKGCONFIG=ON make install -j2 @@ -324,7 +326,9 @@ make install -j2 mkdir -p "${RootDir}/urdfdom_headers/build" cd "${RootDir}/urdfdom_headers/build" -cmake "${RootDir}/urdfdom_headers" -Wno-dev -DCMAKE_INSTALL_PREFIX="${InstallDir}" -DCMAKE_BUILD_TYPE="$BUILD_TYPE" +cmake "${RootDir}/urdfdom_headers" -Wno-dev -DCMAKE_CXX_STANDARD=17 \ + -DCMAKE_C_COMPILER="${CMAKE_C_COMPILER}" -DCMAKE_CXX_COMPILER="${CMAKE_CXX_COMPILER}" \ + -DCMAKE_INSTALL_PREFIX="${InstallDir}" -DCMAKE_BUILD_TYPE="$BUILD_TYPE" make install -j2 ################################## Build and install urdfdom ########################################### @@ -437,8 +441,8 @@ cmake "${RootDir}/pinocchio" -Wno-dev -DCMAKE_CXX_STANDARD=17 \ -DBUILD_WITH_AUTODIFF_SUPPORT=ON -DBUILD_WITH_CODEGEN_SUPPORT=ON -DBUILD_WITH_CASADI_SUPPORT=OFF \ -DBUILD_WITH_OPENMP_SUPPORT=OFF -DGENERATE_PYTHON_STUBS=OFF -DBUILD_TESTING=OFF -DINSTALL_DOCUMENTATION=OFF \ -DCMAKE_CXX_FLAGS_RELEASE_INIT="" -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -DBOOST_BIND_GLOBAL_PLACEHOLDERS $( - ) -Wno-uninitialized -Wno-type-limits -Wno-deprecated-declarations -Wno-unused-local-typedefs $( - ) -Wno-extra -Wno-unknown-warning-option -Wno-unknown-warning" -DCMAKE_BUILD_TYPE="$BUILD_TYPE" + ) -Wno-uninitialized -Wno-type-limits -Wno-unused-local-typedefs -Wno-extra $( + ) -Wno-unknown-warning-option -Wno-unknown-warning" -DCMAKE_BUILD_TYPE="$BUILD_TYPE" make install -j2 # Copy cmake configuration files for cppad and cppadcodegen diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 56423895d..56bface0f 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -61,8 +61,6 @@ set(SRC "${CMAKE_CURRENT_SOURCE_DIR}/src/stepper/abstract_runge_kutta_stepper.cc" "${CMAKE_CURRENT_SOURCE_DIR}/src/stepper/runge_kutta4_stepper.cc" "${CMAKE_CURRENT_SOURCE_DIR}/src/stepper/runge_kutta_dopri_stepper.cc" - "${CMAKE_CURRENT_SOURCE_DIR}/src/engine/system.cc" - "${CMAKE_CURRENT_SOURCE_DIR}/src/engine/engine_multi_robot.cc" "${CMAKE_CURRENT_SOURCE_DIR}/src/engine/engine.cc" ) diff --git a/core/examples/double_pendulum/double_pendulum.cc b/core/examples/double_pendulum/double_pendulum.cc index e24710588..818ba35ba 100644 --- a/core/examples/double_pendulum/double_pendulum.cc +++ b/core/examples/double_pendulum/double_pendulum.cc @@ -35,7 +35,7 @@ void internalDynamics(double /* t */, { } -bool callback(double /* t */, const Eigen::VectorXd & /* q */, const Eigen::VectorXd & /* v */) +bool callback() { return true; } @@ -81,8 +81,8 @@ int main(int /* argc */, char * /* argv */[]) } // Instantiate and configuration the controller - auto controller = std::make_shared>(computeCommand, internalDynamics); - controller->initialize(robot); + robot->setController( + std::make_shared>(computeCommand, internalDynamics)); // Instantiate and configuration the engine Engine engine{}; @@ -119,7 +119,7 @@ int main(int /* argc */, char * /* argv */[]) boost::get(contactsOptions.at("transitionEps")) = 0.001; boost::get(contactsOptions.at("transitionVelocity")) = 0.01; engine.setOptions(simuOptions); - engine.initialize(robot, controller, callback); + engine.addRobot(robot); std::cout << "Initialization: " << timer.toc() << "ms" << std::endl; // ===================================================================== @@ -134,7 +134,7 @@ int main(int /* argc */, char * /* argv */[]) // Run simulation timer.tic(); - engine.simulate(tf, q0, v0); + engine.simulate(tf, q0, v0, std::nullopt, false, callback); std::cout << "Simulation: " << timer.toc() << "ms" << std::endl; // Write the log file diff --git a/core/examples/external_project/double_pendulum.cc b/core/examples/external_project/double_pendulum.cc index 47619e947..981579859 100644 --- a/core/examples/external_project/double_pendulum.cc +++ b/core/examples/external_project/double_pendulum.cc @@ -77,12 +77,12 @@ int main(int argc, char * argv[]) } // Instantiate the controller - auto controller = std::make_shared>(computeCommand, internalDynamics); - controller->initialize(robot); + robot->setController( + std::make_shared>(computeCommand, internalDynamics)); // Instantiate the engine Engine engine{}; - engine.initialize(robot, controller, callback); + engine.addRobot(robot); std::cout << "Initialization: " << timer.toc() << "ms" << std::endl; // ===================================================================== @@ -97,7 +97,7 @@ int main(int argc, char * argv[]) // Run simulation timer.tic(); - engine.simulate(tf, q0, v0); + engine.simulate(tf, q0, v0, std::nullopt, callback); std::cout << "Simulation: " << timer.toc() << "ms" << std::endl; // Write the log file diff --git a/core/include/jiminy/core/control/abstract_controller.h b/core/include/jiminy/core/control/abstract_controller.h index 0a7ba30ca..df61d0ebc 100644 --- a/core/include/jiminy/core/control/abstract_controller.h +++ b/core/include/jiminy/core/control/abstract_controller.h @@ -16,7 +16,6 @@ namespace jiminy class TelemetrySender; class TelemetryData; class Robot; - class Engine; /// \brief Generic interface for any controller. /// @@ -53,7 +52,10 @@ namespace jiminy explicit AbstractController() noexcept; virtual ~AbstractController(); - /// \brief Set the parameters of the controller. + /// \brief Initialize the internal state of the controller for a given robot. + /// + /// \details This method can be called multiple times with different robots. The internal + /// state of the controller will be systematically overwritten. /// /// \param[in] robot Robot virtual void initialize(std::weak_ptr robot); diff --git a/core/include/jiminy/core/engine/engine.h b/core/include/jiminy/core/engine/engine.h index 1d78cf95a..b7f4b7d81 100644 --- a/core/include/jiminy/core/engine/engine.h +++ b/core/include/jiminy/core/engine/engine.h @@ -1,114 +1,858 @@ -#ifndef JIMINY_ENGINE_H -#define JIMINY_ENGINE_H +#ifndef JIMINY_ENGINE_MULTIROBOT_H +#define JIMINY_ENGINE_MULTIROBOT_H +#include #include +#include #include "jiminy/core/fwd.h" -#include "jiminy/core/engine/engine_multi_robot.h" +#include "jiminy/core/utilities/helpers.h" // `Timer` +#include "jiminy/core/utilities/random.h" // `PCG32` +#include "jiminy/core/robot/model.h" namespace jiminy { - class JIMINY_DLLAPI Engine : public EngineMultiRobot + inline constexpr std::string_view ENGINE_TELEMETRY_NAMESPACE{"HighLevelController"}; + + enum class JIMINY_DLLAPI ContactModelType : uint8_t + { + UNSUPPORTED = 0, + SPRING_DAMPER = 1, + CONSTRAINT = 2 + }; + + enum class JIMINY_DLLAPI ConstraintSolverType : uint8_t + { + UNSUPPORTED = 0, + PGS = 1 // Projected Gauss-Seidel + }; + + const std::map CONTACT_MODELS_MAP{ + {"spring_damper", ContactModelType::SPRING_DAMPER}, + {"constraint", ContactModelType::CONSTRAINT}, + }; + + const std::map CONSTRAINT_SOLVERS_MAP{ + {"PGS", ConstraintSolverType::PGS}}; + + const std::set STEPPERS{"euler_explicit", "runge_kutta_4", "runge_kutta_dopri5"}; + + class Robot; + class AbstractConstraintSolver; + class AbstractConstraintBase; + class AbstractController; + class AbstractStepper; + class LockGuardLocal; + class TelemetryData; + class TelemetryRecorder; + class TelemetrySender; + struct LogData; + + // ******************************** External force functors ******************************** // + + using ProfileForceFunction = std::function; + + struct JIMINY_DLLAPI ProfileForce { + public: + // FIXME: Designated aggregate initialization without constructors when moving to C++20 + explicit ProfileForce() = default; + explicit ProfileForce(const std::string & frameNameIn, + pinocchio::FrameIndex frameIndexIn, + double updatePeriodIn, + const ProfileForceFunction & forceFuncIn) noexcept; + + public: + std::string frameName{}; + pinocchio::FrameIndex frameIndex{0}; + double updatePeriod{0.0}; + pinocchio::Force force{pinocchio::Force::Zero()}; + ProfileForceFunction func{}; + }; + + struct JIMINY_DLLAPI ImpulseForce + { + public: + // FIXME: Designated aggregate initialization without constructors when moving to C++20 + explicit ImpulseForce() = default; + explicit ImpulseForce(const std::string & frameNameIn, + pinocchio::FrameIndex frameIndexIn, + double tIn, + double dtIn, + const pinocchio::Force & forceIn) noexcept; + + + public: + std::string frameName{}; + pinocchio::FrameIndex frameIndex{0}; + double t{0.0}; + double dt{0.0}; + pinocchio::Force force{}; + }; + + using CouplingForceFunction = + std::function; + + struct CouplingForce + { + public: + // FIXME: Designated aggregate initialization without constructors when moving to C++20 + explicit CouplingForce() = default; + explicit CouplingForce(const std::string & robotName1In, + std::ptrdiff_t robotIndex1In, + const std::string & robotName2In, + std::ptrdiff_t robotIndex2In, + const std::string & frameName1In, + pinocchio::FrameIndex frameIndex1In, + const std::string & frameName2In, + pinocchio::FrameIndex frameIndex2In, + const CouplingForceFunction & forceFunIn) noexcept; + + public: + std::string robotName1{}; + std::ptrdiff_t robotIndex1{-1}; + std::string robotName2{}; + std::ptrdiff_t robotIndex2{-1}; + std::string frameName1{}; + pinocchio::FrameIndex frameIndex1{0}; + std::string frameName2{}; + pinocchio::FrameIndex frameIndex2{0}; + CouplingForceFunction func{}; + }; + + using ProfileForceVector = std::vector; + using ImpulseForceVector = std::vector; + using CouplingForceVector = std::vector; + + // ************************************** Robot state ************************************** // + + struct JIMINY_DLLAPI RobotState + { + public: + void initialize(const Robot & robot); + bool getIsInitialized() const; + + void clear(); + + public: + Eigen::VectorXd q{}; + Eigen::VectorXd v{}; + Eigen::VectorXd a{}; + Eigen::VectorXd command{}; + Eigen::VectorXd u{}; + Eigen::VectorXd uMotor{}; + Eigen::VectorXd uInternal{}; + Eigen::VectorXd uCustom{}; + ForceVector fExternal{}; + + private: + bool isInitialized_{false}; + }; + + // *************************************** Robot data ************************************** // + + struct JIMINY_DLLAPI RobotData + { + public: + DISABLE_COPY(RobotData) + + /* Must move all definitions in source files to avoid compilation failure due to incomplete + destructor for objects managed by `unique_ptr` member variable with MSVC compiler. + See: https://stackoverflow.com/a/9954553 + https://developercommunity.visualstudio.com/t/unique-ptr-cant-delete-an-incomplete-type/1371585 + */ + explicit RobotData(); + explicit RobotData(RobotData &&); + RobotData & operator=(RobotData &&); + ~RobotData(); + + public: + std::unique_ptr robotLock{nullptr}; + + ProfileForceVector profileForces{}; + ImpulseForceVector impulseForces{}; + /// \brief Sorted list without repetitions of all the start/stop times of impulse forces. + std::set impulseForceBreakpoints{}; + /// \brief Time of the next breakpoint associated with the impulse forces. + std::set::const_iterator impulseForceBreakpointNextIt{}; + /// \brief Set of flags tracking whether each force is active. + /// + /// \details This flag is used to handle t-, t+ properly. Without it, it is impossible to + /// determine at time t if the force is active or not. + std::vector isImpulseForceActiveVec{}; + + uint32_t successiveSolveFailed{0}; + std::unique_ptr constraintSolver{nullptr}; + /// \brief Store copy of constraints register for fast access. + ConstraintTree constraints{}; + /// \brief Contact forces for each contact frames in local frame. + ForceVector contactFrameForces{}; + /// \brief Contact forces for each geometries of each collision bodies in local frame. + vector_aligned_t collisionBodiesForces{}; + /// \brief Jacobian of the joints in local frame. Used for computing `data.u`. + std::vector jointJacobians{}; + + std::vector logPositionFieldnames{}; + std::vector logVelocityFieldnames{}; + std::vector logAccelerationFieldnames{}; + std::vector logForceExternalFieldnames{}; + std::vector logCommandFieldnames{}; + std::vector logMotorEffortFieldnames{}; + std::string logEnergyFieldname{}; + + /// \brief Internal buffer with the state for the integration loop. + RobotState state{}; + /// \brief Internal state for the integration loop at the end of the previous iteration. + RobotState statePrev{}; + }; + + // ************************************* Stepper state ************************************* // + + struct JIMINY_DLLAPI StepperState + { + public: + void reset(double dtInit, + const std::vector & qSplitInit, + const std::vector & vSplitInit, + const std::vector & aSplitInit) + { + iter = 0U; + iterFailed = 0U; + t = 0.0; + tPrev = 0.0; + dt = dtInit; + dtLargest = dtInit; + dtLargestPrev = dtInit; + tError = 0.0; + qSplit = qSplitInit; + vSplit = vSplitInit; + aSplit = aSplitInit; + } + + public: + uint32_t iter{0U}; + uint32_t iterFailed{0U}; + double t{0.0}; + double tPrev{0.0}; + /// \brief Internal buffer used for Kahan algorithm storing the residual sum of errors. + double tError{0.0}; + double dt{0.0}; + double dtLargest{0.0}; + double dtLargestPrev{0.0}; + std::vector qSplit{}; + std::vector vSplit{}; + std::vector aSplit{}; + }; + + // ************************************ Engine *********************************** // + + // Early termination callback functor + using AbortSimulationFunction = std::function; + + class JIMINY_DLLAPI Engine + { + public: + GenericConfig getDefaultConstraintOptions() + { + GenericConfig config; + config["solver"] = std::string{"PGS"}; // ["PGS",] + /// \brief Relative inverse damping wrt diagonal of J.Minv.J.t. + /// + /// \details 0.0 enforces the minimum absolute regularizer. + config["regularization"] = 1.0e-3; + config["successiveSolveFailedMax"] = 100U; + + return config; + }; + + GenericConfig getDefaultContactOptions() + { + GenericConfig config; + config["model"] = std::string{"constraint"}; // ["spring_damper", "constraint"] + config["stiffness"] = 1.0e6; + config["damping"] = 2.0e3; + config["friction"] = 1.0; + config["torsion"] = 0.0; + config["transitionEps"] = 1.0e-3; // [m] + config["transitionVelocity"] = 1.0e-2; // [m.s-1] + config["stabilizationFreq"] = 20.0; // [s-1]: 0.0 to disable + + return config; + }; + + GenericConfig getDefaultJointOptions() + { + GenericConfig config; + config["boundStiffness"] = 1.0e7; + config["boundDamping"] = 1.0e4; + + return config; + }; + + GenericConfig getDefaultWorldOptions() + { + GenericConfig config; + config["gravity"] = (Eigen::VectorXd(6) << 0.0, 0.0, -9.81, 0.0, 0.0, 0.0).finished(); + config["groundProfile"] = HeightmapFunction( + [](const Eigen::Vector2d & /* xy */, + double & height, + Eigen::Vector3d & normal) -> void + { + height = 0.0; + normal = Eigen::Vector3d::UnitZ(); + }); + + return config; + }; + + GenericConfig getDefaultStepperOptions() + { + GenericConfig config; + config["verbose"] = false; + config["randomSeedSeq"] = VectorX::Zero(1).eval(); + /// \details Must be either "runge_kutta_dopri5", "runge_kutta_4" or "euler_explicit". + config["odeSolver"] = std::string{"runge_kutta_dopri5"}; + config["tolAbs"] = 1.0e-5; + config["tolRel"] = 1.0e-4; + config["dtMax"] = SIMULATION_MAX_TIMESTEP; + config["dtRestoreThresholdRel"] = 0.2; + config["successiveIterFailedMax"] = 1000U; + config["iterMax"] = 0U; // <= 0: disable + config["timeout"] = 0.0; // <= 0.0: disable + config["sensorsUpdatePeriod"] = 0.0; + config["controllerUpdatePeriod"] = 0.0; + config["logInternalStepperSteps"] = false; + + return config; + }; + + GenericConfig getDefaultTelemetryOptions() + { + GenericConfig config; + config["isPersistent"] = false; + config["enableConfiguration"] = true; + config["enableVelocity"] = true; + config["enableAcceleration"] = true; + config["enableForceExternal"] = false; + config["enableCommand"] = true; + config["enableMotorEffort"] = true; + config["enableEnergy"] = true; + return config; + }; + + GenericConfig getDefaultEngineOptions() + { + GenericConfig config; + config["telemetry"] = getDefaultTelemetryOptions(); + config["stepper"] = getDefaultStepperOptions(); + config["world"] = getDefaultWorldOptions(); + config["joints"] = getDefaultJointOptions(); + config["constraints"] = getDefaultConstraintOptions(); + config["contacts"] = getDefaultContactOptions(); + + return config; + }; + + struct ConstraintOptions + { + const std::string solver; + const double regularization; + const uint32_t successiveSolveFailedMax; + + ConstraintOptions(const GenericConfig & options) : + solver{boost::get(options.at("solver"))}, + regularization{boost::get(options.at("regularization"))}, + successiveSolveFailedMax{boost::get(options.at("successiveSolveFailedMax"))} + { + } + }; + + struct ContactOptions + { + const std::string model; + const double stiffness; + const double damping; + const double friction; + const double torsion; + const double transitionEps; + const double transitionVelocity; + const double stabilizationFreq; + + ContactOptions(const GenericConfig & options) : + model{boost::get(options.at("model"))}, + stiffness{boost::get(options.at("stiffness"))}, + damping{boost::get(options.at("damping"))}, + friction{boost::get(options.at("friction"))}, + torsion{boost::get(options.at("torsion"))}, + transitionEps{boost::get(options.at("transitionEps"))}, + transitionVelocity{boost::get(options.at("transitionVelocity"))}, + stabilizationFreq{boost::get(options.at("stabilizationFreq"))} + { + } + }; + + struct JointOptions + { + const double boundStiffness; + const double boundDamping; + + JointOptions(const GenericConfig & options) : + boundStiffness{boost::get(options.at("boundStiffness"))}, + boundDamping{boost::get(options.at("boundDamping"))} + { + } + }; + + struct WorldOptions + { + const Eigen::VectorXd gravity; + const HeightmapFunction groundProfile; + + WorldOptions(const GenericConfig & options) : + gravity{boost::get(options.at("gravity"))}, + groundProfile{boost::get(options.at("groundProfile"))} + { + } + }; + + struct StepperOptions + { + const bool verbose; + const VectorX randomSeedSeq; + const std::string odeSolver; + const double tolAbs; + const double tolRel; + const double dtMax; + const double dtRestoreThresholdRel; + const uint32_t successiveIterFailedMax; + const uint32_t iterMax; + const double timeout; + const double sensorsUpdatePeriod; + const double controllerUpdatePeriod; + const bool logInternalStepperSteps; + + StepperOptions(const GenericConfig & options) : + verbose{boost::get(options.at("verbose"))}, + randomSeedSeq{boost::get>(options.at("randomSeedSeq"))}, + odeSolver{boost::get(options.at("odeSolver"))}, + tolAbs{boost::get(options.at("tolAbs"))}, + tolRel{boost::get(options.at("tolRel"))}, + dtMax{boost::get(options.at("dtMax"))}, + dtRestoreThresholdRel{boost::get(options.at("dtRestoreThresholdRel"))}, + successiveIterFailedMax{boost::get(options.at("successiveIterFailedMax"))}, + iterMax{boost::get(options.at("iterMax"))}, + timeout{boost::get(options.at("timeout"))}, + sensorsUpdatePeriod{boost::get(options.at("sensorsUpdatePeriod"))}, + controllerUpdatePeriod{boost::get(options.at("controllerUpdatePeriod"))}, + logInternalStepperSteps{boost::get(options.at("logInternalStepperSteps"))} + { + } + }; + + struct TelemetryOptions + { + const bool isPersistent; + const bool enableConfiguration; + const bool enableVelocity; + const bool enableAcceleration; + const bool enableForceExternal; + const bool enableCommand; + const bool enableMotorEffort; + const bool enableEnergy; + + TelemetryOptions(const GenericConfig & options) : + isPersistent{boost::get(options.at("isPersistent"))}, + enableConfiguration{boost::get(options.at("enableConfiguration"))}, + enableVelocity{boost::get(options.at("enableVelocity"))}, + enableAcceleration{boost::get(options.at("enableAcceleration"))}, + enableForceExternal{boost::get(options.at("enableForceExternal"))}, + enableCommand{boost::get(options.at("enableCommand"))}, + enableMotorEffort{boost::get(options.at("enableMotorEffort"))}, + enableEnergy{boost::get(options.at("enableEnergy"))} + { + } + }; + + struct EngineOptions + { + const TelemetryOptions telemetry; + const StepperOptions stepper; + const WorldOptions world; + const JointOptions joints; + const ConstraintOptions constraints; + const ContactOptions contacts; + + EngineOptions(const GenericConfig & options) : + telemetry{boost::get(options.at("telemetry"))}, + stepper{boost::get(options.at("stepper"))}, + world{boost::get(options.at("world"))}, + joints{boost::get(options.at("joints"))}, + constraints{boost::get(options.at("constraints"))}, + contacts{boost::get(options.at("contacts"))} + { + } + }; + public: DISABLE_COPY(Engine) public: - explicit Engine() = default; - ~Engine() = default; + explicit Engine() noexcept; + ~Engine(); + + void addRobot(std::shared_ptr robot); + void removeRobot(const std::string & robotName); + + /// \brief Add a force linking both robots together. + /// + /// \details This function registers a callback function that links both robots by a given + /// force. This function must return the force that the second robots applies to + /// the first robot, in the global frame of the first frame, i.e. expressed at + /// the origin of the first frame, in word coordinates. + /// + /// \param[in] robotName1 Name of the robot receiving the force. + /// \param[in] robotName2 Name of the robot applying the force. + /// \param[in] frameName1 Frame on the first robot where the force is applied. + /// \param[in] frameName2 Frame on the second robot where the opposite force is applied. + /// \param[in] forceFunc Callback function returning the force that robotName2 applies on + /// robotName1, in the global frame of frameName1. + void registerCouplingForce(const std::string & robotName1, + const std::string & robotName2, + const std::string & frameName1, + const std::string & frameName2, + const CouplingForceFunction & forceFunc); + void registerViscoelasticDirectionalCouplingForce(const std::string & robotName1, + const std::string & robotName2, + const std::string & frameName1, + const std::string & frameName2, + double stiffness, + double damping, + double restLength = 0.0); + void registerViscoelasticDirectionalCouplingForce(const std::string & robotName, + const std::string & frameName1, + const std::string & frameName2, + double stiffness, + double damping, + double restLength = 0.0); - void initialize(std::shared_ptr robot, - std::shared_ptr controller, - const AbortSimulationFunction & callback); - void initialize(std::shared_ptr robot, const AbortSimulationFunction & callback); + /// \brief 6-DoFs spring-damper coupling force modelling a flexible bushing. + /// + /// \details The spring-damper forces are computed at a frame being the linear + /// interpolation (according on double-geodesic) between frame 1 and 2 by a factor + /// alpha. In particular, alpha = 0.0, 0.5, and 1.0 corresponds to frame 1, + /// halfway point, and frame 2. + /// + /// \see See official drake documentation: + /// https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_linear_bushing_roll_pitch_yaw.html + void registerViscoelasticCouplingForce(const std::string & robotName1, + const std::string & robotName2, + const std::string & frameName1, + const std::string & frameName2, + const Vector6d & stiffness, + const Vector6d & damping, + double alpha = 0.5); + void registerViscoelasticCouplingForce(const std::string & robotName, + const std::string & frameName1, + const std::string & frameName2, + const Vector6d & stiffness, + const Vector6d & damping, + double alpha = 0.5); + void removeCouplingForces(const std::string & robotName1, const std::string & robotName2); + void removeCouplingForces(const std::string & robotName); + void removeCouplingForces(); - void setController(std::shared_ptr controller); + const CouplingForceVector & getCouplingForces() const; + + void removeAllForces(); - /* Forbid direct usage of these methods since it does not make sense for single robot - engine (every overloads are affected at once). */ - [[noreturn]] void addSystem(const std::string & systemName, - std::shared_ptr robot, - std::shared_ptr controller); - [[noreturn]] void removeSystem(const std::string & systemName); + /// \brief Reset the engine and all the robots, which includes hardware and controller for + /// each of them. + /// + /// \details This method is made to be called in between simulations, to allow registering + /// of new variables to the telemetry, and reset the random number generators. + /// + /// \param[in] resetRandomNumbers Whether to reset the random number generators. + /// \param[in] removeAllForce Whether to remove registered external forces. + void reset(bool resetRandomNumbers = false, bool removeAllForce = false); - /// \brief Reset the engine and compute initial state. + /// \brief Start the simulation /// - /// \details This function does NOT reset the engine, robot and controller. - /// It is up to the user to do so, by calling `reset` method first. + /// \warning This function calls `reset` internally only if necessary, namely if it was not + /// done manually at some point after stopping the previous simulation if any. /// - /// \param[in] qInit Initial configuration. - /// \param[in] vInit Initial velocity. - /// \param[in] aInit Initial acceleration. + /// \param[in] qInit Initial configuration of every robots. + /// \param[in] vInit Initial velocity of every robots. + /// \param[in] aInit Initial acceleration of every robots. /// Optional: Zero by default. - /// \param[in] isStateTheoretical Specify if the initial state is associated with the - /// current or theoretical model. + void start( + const std::map & qInit, + const std::map & vInit, + const std::optional> & aInit = std::nullopt); + void start(const Eigen::VectorXd & qInit, const Eigen::VectorXd & vInit, const std::optional & aInit = std::nullopt, bool isStateTheoretical = false); + /// \brief Integrate robot dynamics from current state for a given duration. + /// + /// \details Internally, the integrator may perform multiple steps inside in the interval. + /// + /// \param[in] stepSize Duration for which to integrate. -1 for default duration, ie until + /// next breakpoint if any, or 'engine_options["stepper"]["dtMax"]'. + void step(double stepSize = -1); + + /// \brief Stop the simulation. + /// + /// \details It releases the lock on the robot and the telemetry, so that it is possible + /// again to update the robot (for example to update the options, add or remove + /// sensors...) and to register new variables or forces. + void stop(); + /// \brief Run a simulation of duration tEnd, starting at xInit. /// - /// \param[in] tEnd End time, i.e. amount of time to simulate. - /// \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. - void simulate(double tEnd, - const Eigen::VectorXd & qInit, - const Eigen::VectorXd & vInit, - const std::optional & aInit = std::nullopt, - bool isStateTheoretical = false); - - void registerImpulseForce( - const std::string & frameName, double t, double dt, const pinocchio::Force & force); - void registerProfileForce(const std::string & frameName, + /// \param[in] tEnd Duration of the simulation. + /// \param[in] qInit Initial configuration of every robots, i.e. at t=0.0. + /// \param[in] vInit Initial velocity of every robots, i.e. at t=0.0. + /// \param[in] aInit Initial acceleration of every robots, i.e. at t=0.0. + /// Optional: Zero by default. + /// \param[in] callback Callable that can be specified to abort simulation. It will be + /// evaluated after every simulation step. Abort if false is returned. + void simulate( + double tEnd, + const std::map & qInit, + const std::map & vInit, + const std::optional> & aInit = std::nullopt, + const AbortSimulationFunction & callback = []() { return true; }); + + void simulate( + double tEnd, + const Eigen::VectorXd & qInit, + const Eigen::VectorXd & vInit, + const std::optional & aInit = std::nullopt, + bool isStateTheoretical = false, + const AbortSimulationFunction & callback = []() { return true; }); + + /// \brief Apply an impulse force on a frame for a given duration at the desired time. + /// + /// \warning The force must be given in the world frame. + void registerImpulseForce(const std::string & robotName, + const std::string & frameName, + double t, + double dt, + const pinocchio::Force & force); + /// \brief Apply an external force profile on a frame. + /// + /// \details It can be either time-continuous or discrete. The force can be time- and + /// state-dependent, and must be given in the world frame. + void registerProfileForce(const std::string & robotName, + const std::string & frameName, const ProfileForceFunction & forceFunc, double updatePeriod = 0.0); - // Redefined to leverage C++ name hiding of overloaded base methods in derived class + void removeImpulseForces(const std::string & robotName); + void removeProfileForces(const std::string & robotName); void removeImpulseForces(); void removeProfileForces(); - const ImpulseForceVector & getImpulseForces() const; - const ProfileForceVector & getProfileForces() const; + const ImpulseForceVector & getImpulseForces(const std::string & robotName) const; + const ProfileForceVector & getProfileForces(const std::string & robotName) const; - void registerCouplingForce(const std::string & frameName1, - const std::string & frameName2, - const ProfileForceFunction & forceFunc); - void registerViscoelasticCouplingForce(const std::string & frameName1, - const std::string & frameName2, - const Vector6d & stiffness, - const Vector6d & damping, - double alpha = 0.5); - void registerViscoelasticDirectionalCouplingForce(const std::string & frameName1, - const std::string & frameName2, - double stiffness, - double damping, - double restLength = 0.0); + GenericConfig getOptions() const noexcept; + void setOptions(const GenericConfig & engineOptions); + bool getIsTelemetryConfigured() const; + std::shared_ptr & getRobot(const std::string & robotName); + std::ptrdiff_t getRobotIndex(const std::string & robotName) const; + const RobotState & getRobotState(const std::string & robotName) const; + const StepperState & getStepperState() const; + const bool & getIsSimulationRunning() const; // return const reference on purpose + static double getSimulationDurationMax(); + static double getTelemetryTimeUnit(); - void removeCouplingForces(); + static void computeForwardKinematics(std::shared_ptr & robot, + const Eigen::VectorXd & q, + const Eigen::VectorXd & v, + const Eigen::VectorXd & a); + void computeRobotsDynamics(double t, + const std::vector & qSplit, + const std::vector & vSplit, + std::vector & aSplit, + bool isStateUpToDate = false); - void removeAllForces(); + protected: + void configureTelemetry(); + void updateTelemetry(); - bool getIsInitialized() const; - System & getSystem(); - std::shared_ptr getRobot(); - std::shared_ptr getController(); - const SystemState & getSystemState() const; + void syncStepperStateWithRobots(); + void syncRobotsStateWithStepper(bool isStateUpToDate = false); + + + /// \brief Compute the force resulting from ground contact on a given body. + /// + /// \param[in] robot Robot for which to perform computation. + /// \param[in] collisionPairIndex Index of the collision pair associated with the body. + /// + /// \returns Contact force, at parent joint, in the local frame. + void computeContactDynamicsAtBody( + const std::shared_ptr & robot, + const pinocchio::PairIndex & collisionPairIndex, + std::shared_ptr & contactConstraint, + pinocchio::Force & fextLocal) const; + + /// \brief Compute the force resulting from ground contact on a given frame. + /// + /// \param[in] robot Robot for which to perform computation. + /// \param[in] frameIndex Index of the frame in contact. + /// + /// \returns Contact force, at parent joint, in the local frame. + void computeContactDynamicsAtFrame( + const std::shared_ptr & robot, + pinocchio::FrameIndex frameIndex, + std::shared_ptr & collisionConstraint, + pinocchio::Force & fextLocal) const; + + /// \brief Compute the ground reaction force for a given normal direction and depth. + pinocchio::Force computeContactDynamics(const Eigen::Vector3d & nGround, + double depth, + const Eigen::Vector3d & vContactInWorld) const; + + void computeCommand(std::shared_ptr & robot, + double t, + const Eigen::VectorXd & q, + const Eigen::VectorXd & v, + Eigen::VectorXd & command); + void computeInternalDynamics(const std::shared_ptr & robot, + RobotData & robotData, + double t, + const Eigen::VectorXd & q, + const Eigen::VectorXd & v, + Eigen::VectorXd & uInternal) const; + void computeCollisionForces(const std::shared_ptr & robot, + RobotData & robotData, + ForceVector & fext, + bool isStateUpToDate = false) const; + void computeExternalForces(const std::shared_ptr & robot, + RobotData & robotData, + double t, + const Eigen::VectorXd & q, + const Eigen::VectorXd & v, + ForceVector & fext); + void computeCouplingForces(double t, + const std::vector & qSplit, + const std::vector & vSplit); + void computeAllTerms(double t, + const std::vector & qSplit, + const std::vector & vSplit, + bool isStateUpToDate = false); + + /// \brief Compute robot acceleration from current robot state. + /// + /// \details This function performs forward dynamics computation, either with kinematic + /// constraints (using Lagrange multiplier for computing the forces) or + /// unconstrained (aba). + /// + /// \param[in] robot Robot for which to compute the dynamics. + /// \param[in] q Joint position. + /// \param[in] v Joint velocity. + /// \param[in] u Joint effort. + /// \param[in] fext External forces applied on the robot. + /// + /// \return Robot acceleration. + const Eigen::VectorXd & computeAcceleration(std::shared_ptr & robot, + RobotData & robotData, + const Eigen::VectorXd & q, + const Eigen::VectorXd & v, + const Eigen::VectorXd & u, + ForceVector & fext, + bool isStateUpToDate = false, + bool ignoreBounds = false); + + public: + std::shared_ptr getLog(); + + static LogData readLog(const std::string & filename, const std::string & format); + + void writeLog(const std::string & filename, const std::string & format); private: - void initializeImpl(std::shared_ptr robot, - std::shared_ptr controller, - const AbortSimulationFunction & callback); + template + class JointCollectionTpl, + typename ConfigVectorType, + typename TangentVectorType> + inline Scalar + kineticEnergy(const pinocchio::ModelTpl & model, + pinocchio::DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + bool update_kinematics); + template + class JointCollectionTpl, + typename ConfigVectorType, + typename TangentVectorType1, + typename TangentVectorType2, + typename ForceDerived> + inline const typename pinocchio::DataTpl:: + TangentVectorType & + rnea(const pinocchio::ModelTpl & model, + pinocchio::DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & a, + const vector_aligned_t & fext); + template + class JointCollectionTpl, + typename ConfigVectorType, + typename TangentVectorType1, + typename TangentVectorType2, + typename ForceDerived> + inline const typename pinocchio::DataTpl:: + TangentVectorType & + aba(const pinocchio::ModelTpl & model, + pinocchio::DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & tau, + const vector_aligned_t & fext); + + public: + std::unique_ptr engineOptions_{nullptr}; + std::vector> robots_{}; protected: - bool isInitialized_; - Robot * robot_; - AbstractController * controller_; + bool isTelemetryConfigured_{false}; + bool isSimulationRunning_{false}; + GenericConfig engineOptionsGeneric_{}; + PCG32 generator_; + + private: + Timer timer_{}; + ContactModelType contactModel_{ContactModelType::UNSUPPORTED}; + std::unique_ptr telemetrySender_; + std::shared_ptr telemetryData_; + std::unique_ptr telemetryRecorder_; + std::unique_ptr stepper_{nullptr}; + double stepperUpdatePeriod_{INF}; + StepperState stepperState_{}; + vector_aligned_t robotDataVec_{}; + CouplingForceVector couplingForces_{}; + vector_aligned_t contactForcesPrev_{}; + vector_aligned_t fPrev_{}; + vector_aligned_t aPrev_{}; + std::vector energy_{}; + std::shared_ptr logData_{nullptr}; }; } -#endif // end of JIMINY_ENGINE_H +#endif // JIMINY_ENGINE_MULTIROBOT_H diff --git a/core/include/jiminy/core/engine/engine_multi_robot.h b/core/include/jiminy/core/engine/engine_multi_robot.h deleted file mode 100644 index 44a0c39e6..000000000 --- a/core/include/jiminy/core/engine/engine_multi_robot.h +++ /dev/null @@ -1,680 +0,0 @@ -#ifndef JIMINY_ENGINE_MULTIROBOT_H -#define JIMINY_ENGINE_MULTIROBOT_H - -#include -#include - -#include "jiminy/core/fwd.h" -#include "jiminy/core/utilities/helpers.h" // `Timer` -#include "jiminy/core/utilities/random.h" // `PCG32` -#include "jiminy/core/engine/system.h" - - -namespace jiminy -{ - inline constexpr std::string_view ENGINE_TELEMETRY_NAMESPACE{"HighLevelController"}; - - enum class JIMINY_DLLAPI ContactModelType : uint8_t - { - UNSUPPORTED = 0, - SPRING_DAMPER = 1, - CONSTRAINT = 2 - }; - - enum class JIMINY_DLLAPI ConstraintSolverType : uint8_t - { - UNSUPPORTED = 0, - PGS = 1 // Projected Gauss-Seidel - }; - - const std::map CONTACT_MODELS_MAP{ - {"spring_damper", ContactModelType::SPRING_DAMPER}, - {"constraint", ContactModelType::CONSTRAINT}, - }; - - const std::map CONSTRAINT_SOLVERS_MAP{ - {"PGS", ConstraintSolverType::PGS}}; - - const std::set STEPPERS{"euler_explicit", "runge_kutta_4", "runge_kutta_dopri5"}; - - class Robot; - class AbstractConstraintBase; - class AbstractController; - class AbstractStepper; - class TelemetryData; - class TelemetryRecorder; - class TelemetrySender; - struct LogData; - - struct JIMINY_DLLAPI StepperState - { - public: - void reset(double dtInit, - const std::vector & qSplitInit, - const std::vector & vSplitInit, - const std::vector & aSplitInit) - { - iter = 0U; - iterFailed = 0U; - t = 0.0; - tPrev = 0.0; - dt = dtInit; - dtLargest = dtInit; - dtLargestPrev = dtInit; - tError = 0.0; - qSplit = qSplitInit; - vSplit = vSplitInit; - aSplit = aSplitInit; - } - - public: - uint32_t iter{0U}; - uint32_t iterFailed{0U}; - double t{0.0}; - double tPrev{0.0}; - /// \brief Internal buffer used for Kahan algorithm storing the residual sum of errors. - double tError{0.0}; - double dt{0.0}; - double dtLargest{0.0}; - double dtLargestPrev{0.0}; - std::vector qSplit{}; - std::vector vSplit{}; - std::vector aSplit{}; - }; - - class JIMINY_DLLAPI EngineMultiRobot - { - public: - GenericConfig getDefaultConstraintOptions() - { - GenericConfig config; - config["solver"] = std::string{"PGS"}; // ["PGS",] - /// \brief Relative inverse damping wrt diagonal of J.Minv.J.t. - /// - /// \details 0.0 enforces the minimum absolute regularizer. - config["regularization"] = 1.0e-3; - config["successiveSolveFailedMax"] = 100U; - - return config; - }; - - GenericConfig getDefaultContactOptions() - { - GenericConfig config; - config["model"] = std::string{"constraint"}; // ["spring_damper", "constraint"] - config["stiffness"] = 1.0e6; - config["damping"] = 2.0e3; - config["friction"] = 1.0; - config["torsion"] = 0.0; - config["transitionEps"] = 1.0e-3; // [m] - config["transitionVelocity"] = 1.0e-2; // [m.s-1] - config["stabilizationFreq"] = 20.0; // [s-1]: 0.0 to disable - - return config; - }; - - GenericConfig getDefaultJointOptions() - { - GenericConfig config; - config["boundStiffness"] = 1.0e7; - config["boundDamping"] = 1.0e4; - - return config; - }; - - GenericConfig getDefaultWorldOptions() - { - GenericConfig config; - config["gravity"] = (Eigen::VectorXd(6) << 0.0, 0.0, -9.81, 0.0, 0.0, 0.0).finished(); - config["groundProfile"] = HeightmapFunction( - [](const Eigen::Vector2d & /* xy */, - double & height, - Eigen::Vector3d & normal) -> void - { - height = 0.0; - normal = Eigen::Vector3d::UnitZ(); - }); - - return config; - }; - - GenericConfig getDefaultStepperOptions() - { - GenericConfig config; - config["verbose"] = false; - config["randomSeedSeq"] = VectorX::Zero(1).eval(); - /// \details Must be either "runge_kutta_dopri5", "runge_kutta_4" or "euler_explicit". - config["odeSolver"] = std::string{"runge_kutta_dopri5"}; - config["tolAbs"] = 1.0e-5; - config["tolRel"] = 1.0e-4; - config["dtMax"] = SIMULATION_MAX_TIMESTEP; - config["dtRestoreThresholdRel"] = 0.2; - config["successiveIterFailedMax"] = 1000U; - config["iterMax"] = 0U; // <= 0: disable - config["timeout"] = 0.0; // <= 0.0: disable - config["sensorsUpdatePeriod"] = 0.0; - config["controllerUpdatePeriod"] = 0.0; - config["logInternalStepperSteps"] = false; - - return config; - }; - - GenericConfig getDefaultTelemetryOptions() - { - GenericConfig config; - config["isPersistent"] = false; - config["enableConfiguration"] = true; - config["enableVelocity"] = true; - config["enableAcceleration"] = true; - config["enableForceExternal"] = false; - config["enableCommand"] = true; - config["enableMotorEffort"] = true; - config["enableEnergy"] = true; - return config; - }; - - GenericConfig getDefaultEngineOptions() - { - GenericConfig config; - config["telemetry"] = getDefaultTelemetryOptions(); - config["stepper"] = getDefaultStepperOptions(); - config["world"] = getDefaultWorldOptions(); - config["joints"] = getDefaultJointOptions(); - config["constraints"] = getDefaultConstraintOptions(); - config["contacts"] = getDefaultContactOptions(); - - return config; - }; - - struct ConstraintOptions - { - const std::string solver; - const double regularization; - const uint32_t successiveSolveFailedMax; - - ConstraintOptions(const GenericConfig & options) : - solver{boost::get(options.at("solver"))}, - regularization{boost::get(options.at("regularization"))}, - successiveSolveFailedMax{boost::get(options.at("successiveSolveFailedMax"))} - { - } - }; - - struct ContactOptions - { - const std::string model; - const double stiffness; - const double damping; - const double friction; - const double torsion; - const double transitionEps; - const double transitionVelocity; - const double stabilizationFreq; - - ContactOptions(const GenericConfig & options) : - model{boost::get(options.at("model"))}, - stiffness{boost::get(options.at("stiffness"))}, - damping{boost::get(options.at("damping"))}, - friction{boost::get(options.at("friction"))}, - torsion{boost::get(options.at("torsion"))}, - transitionEps{boost::get(options.at("transitionEps"))}, - transitionVelocity{boost::get(options.at("transitionVelocity"))}, - stabilizationFreq{boost::get(options.at("stabilizationFreq"))} - { - } - }; - - struct JointOptions - { - const double boundStiffness; - const double boundDamping; - - JointOptions(const GenericConfig & options) : - boundStiffness{boost::get(options.at("boundStiffness"))}, - boundDamping{boost::get(options.at("boundDamping"))} - { - } - }; - - struct WorldOptions - { - const Eigen::VectorXd gravity; - const HeightmapFunction groundProfile; - - WorldOptions(const GenericConfig & options) : - gravity{boost::get(options.at("gravity"))}, - groundProfile{boost::get(options.at("groundProfile"))} - { - } - }; - - struct StepperOptions - { - const bool verbose; - const VectorX randomSeedSeq; - const std::string odeSolver; - const double tolAbs; - const double tolRel; - const double dtMax; - const double dtRestoreThresholdRel; - const uint32_t successiveIterFailedMax; - const uint32_t iterMax; - const double timeout; - const double sensorsUpdatePeriod; - const double controllerUpdatePeriod; - const bool logInternalStepperSteps; - - StepperOptions(const GenericConfig & options) : - verbose{boost::get(options.at("verbose"))}, - randomSeedSeq{boost::get>(options.at("randomSeedSeq"))}, - odeSolver{boost::get(options.at("odeSolver"))}, - tolAbs{boost::get(options.at("tolAbs"))}, - tolRel{boost::get(options.at("tolRel"))}, - dtMax{boost::get(options.at("dtMax"))}, - dtRestoreThresholdRel{boost::get(options.at("dtRestoreThresholdRel"))}, - successiveIterFailedMax{boost::get(options.at("successiveIterFailedMax"))}, - iterMax{boost::get(options.at("iterMax"))}, - timeout{boost::get(options.at("timeout"))}, - sensorsUpdatePeriod{boost::get(options.at("sensorsUpdatePeriod"))}, - controllerUpdatePeriod{boost::get(options.at("controllerUpdatePeriod"))}, - logInternalStepperSteps{boost::get(options.at("logInternalStepperSteps"))} - { - } - }; - - struct TelemetryOptions - { - const bool isPersistent; - const bool enableConfiguration; - const bool enableVelocity; - const bool enableAcceleration; - const bool enableForceExternal; - const bool enableCommand; - const bool enableMotorEffort; - const bool enableEnergy; - - TelemetryOptions(const GenericConfig & options) : - isPersistent{boost::get(options.at("isPersistent"))}, - enableConfiguration{boost::get(options.at("enableConfiguration"))}, - enableVelocity{boost::get(options.at("enableVelocity"))}, - enableAcceleration{boost::get(options.at("enableAcceleration"))}, - enableForceExternal{boost::get(options.at("enableForceExternal"))}, - enableCommand{boost::get(options.at("enableCommand"))}, - enableMotorEffort{boost::get(options.at("enableMotorEffort"))}, - enableEnergy{boost::get(options.at("enableEnergy"))} - { - } - }; - - struct EngineOptions - { - const TelemetryOptions telemetry; - const StepperOptions stepper; - const WorldOptions world; - const JointOptions joints; - const ConstraintOptions constraints; - const ContactOptions contacts; - - EngineOptions(const GenericConfig & options) : - telemetry{boost::get(options.at("telemetry"))}, - stepper{boost::get(options.at("stepper"))}, - world{boost::get(options.at("world"))}, - joints{boost::get(options.at("joints"))}, - constraints{boost::get(options.at("constraints"))}, - contacts{boost::get(options.at("contacts"))} - { - } - }; - - public: - DISABLE_COPY(EngineMultiRobot) - - public: - explicit EngineMultiRobot() noexcept; - ~EngineMultiRobot(); - - void addSystem(const std::string & systemName, - std::shared_ptr robot, - std::shared_ptr controller, - const AbortSimulationFunction & callback); - void addSystem(const std::string & systemName, - std::shared_ptr robot, - const AbortSimulationFunction & callback); - void removeSystem(const std::string & systemName); - - void setController(const std::string & systemName, - std::shared_ptr controller); - - /// \brief Add a force linking both systems together. - /// - /// \details This function registers a callback function that links both systems by a given - /// force. This function must return the force that the second systems applies to - /// the first system, in the global frame of the first frame, i.e. expressed at - /// the origin of the first frame, in word coordinates. - /// - /// \param[in] systemName1 Name of the system receiving the force. - /// \param[in] systemName2 Name of the system applying the force. - /// \param[in] frameName1 Frame on the first system where the force is applied. - /// \param[in] frameName2 Frame on the second system where the opposite force is applied. - /// \param[in] forceFunc Callback function returning the force that systemName2 applies on - /// systemName1, in the global frame of frameName1. - void registerCouplingForce(const std::string & systemName1, - const std::string & systemName2, - const std::string & frameName1, - const std::string & frameName2, - const CouplingForceFunction & forceFunc); - void registerViscoelasticDirectionalCouplingForce(const std::string & systemName1, - const std::string & systemName2, - const std::string & frameName1, - const std::string & frameName2, - double stiffness, - double damping, - double restLength = 0.0); - void registerViscoelasticDirectionalCouplingForce(const std::string & systemName, - const std::string & frameName1, - const std::string & frameName2, - double stiffness, - double damping, - double restLength = 0.0); - - /// \brief 6-DoFs spring-damper coupling force modelling a flexible bushing. - /// - /// \details The spring-damper forces are computed at a frame being the linear - /// interpolation (according on double-geodesic) between frame 1 and 2 by a factor - /// alpha. In particular, alpha = 0.0, 0.5, and 1.0 corresponds to frame 1, - /// halfway point, and frame 2. - /// - /// \see See official drake documentation: - /// https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_linear_bushing_roll_pitch_yaw.html - void registerViscoelasticCouplingForce(const std::string & systemName1, - const std::string & systemName2, - const std::string & frameName1, - const std::string & frameName2, - const Vector6d & stiffness, - const Vector6d & damping, - double alpha = 0.5); - void registerViscoelasticCouplingForce(const std::string & systemName, - const std::string & frameName1, - const std::string & frameName2, - const Vector6d & stiffness, - const Vector6d & damping, - double alpha = 0.5); - void removeCouplingForces(const std::string & systemName1, - const std::string & systemName2); - void removeCouplingForces(const std::string & systemName); - void removeCouplingForces(); - - const CouplingForceVector & getCouplingForces() const; - - void removeAllForces(); - - /// \brief Reset engine. - /// - /// \details This function resets the engine, the robot and the controller. - /// This method is made to be called in between simulations, to allow registering - /// of new variables to the telemetry, and reset the random number generators. - /// - /// \param[in] resetRandomNumbers Whether to reset the random number generators. - /// \param[in] removeAllForce Whether to remove registered external forces. - void reset(bool resetRandomNumbers = false, bool removeAllForce = false); - - /// \brief Reset the engine and compute initial state. - /// - /// \warning This function does NOT reset the engine, robot and controller. It is up to - /// the user to do so, by calling `reset` method first. - /// - /// \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. - void start( - const std::map & qInit, - const std::map & vInit, - const std::optional> & aInit = std::nullopt); - - /// \brief Integrate system dynamics from current state for a given duration. - /// - /// \details Internally, the integrator may perform multiple steps inside in the interval. - /// - /// \param[in] stepSize Duration for which to integrate. -1 for default duration, ie until - /// next breakpoint if any, or 'engine_options["stepper"]["dtMax"]'. - void step(double stepSize = -1); - - /// \brief Stop the simulation. - /// - /// \details It releases the lock on the robot and the telemetry, so that it is possible - /// again to update the robot (for example to update the options, add or remove - /// sensors...) and to register new variables or forces. - void stop(); - - /// \brief Run a simulation of duration tEnd, starting at xInit. - /// - /// \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. - void simulate( - double tEnd, - const std::map & qInit, - const std::map & vInit, - const std::optional> & aInit = std::nullopt); - - /// \brief Apply an impulse force on a frame for a given duration at the desired time. - /// - /// \warning The force must be given in the world frame. - void registerImpulseForce(const std::string & systemName, - const std::string & frameName, - double t, - double dt, - const pinocchio::Force & force); - /// \brief Apply an external force profile on a frame. - /// - /// \details It can be either time-continuous or discrete. The force can be time- and - /// state-dependent, and must be given in the world frame. - void registerProfileForce(const std::string & systemName, - const std::string & frameName, - const ProfileForceFunction & forceFunc, - double updatePeriod = 0.0); - - void removeImpulseForces(const std::string & systemName); - void removeProfileForces(const std::string & systemName); - void removeImpulseForces(); - void removeProfileForces(); - - const ImpulseForceVector & getImpulseForces(const std::string & systemName) const; - const ProfileForceVector & getProfileForces(const std::string & systemName) const; - - GenericConfig getOptions() const noexcept; - void setOptions(const GenericConfig & engineOptions); - bool getIsTelemetryConfigured() const; - std::vector getSystemNames() const; - System & getSystem(const std::string & systemName); - std::ptrdiff_t getSystemIndex(const std::string & systemName) const; - const SystemState & getSystemState(const std::string & systemName) const; - const StepperState & getStepperState() const; - const bool & getIsSimulationRunning() const; // return const reference on purpose - static double getSimulationDurationMax(); - static double getTelemetryTimeUnit(); - - static void computeForwardKinematics(System & system, - const Eigen::VectorXd & q, - const Eigen::VectorXd & v, - const Eigen::VectorXd & a); - void computeSystemsDynamics(double t, - const std::vector & qSplit, - const std::vector & vSplit, - std::vector & aSplit, - bool isStateUpToDate = false); - - protected: - void configureTelemetry(); - void updateTelemetry(); - - void syncStepperStateWithSystems(); - void syncSystemsStateWithStepper(bool isStateUpToDate = false); - - - /// \brief Compute the force resulting from ground contact on a given body. - /// - /// \param[in] system System for which to perform computation. - /// \param[in] collisionPairIndex Index of the collision pair associated with the body. - /// - /// \returns Contact force, at parent joint, in the local frame. - void computeContactDynamicsAtBody( - const System & system, - const pinocchio::PairIndex & collisionPairIndex, - std::shared_ptr & contactConstraint, - pinocchio::Force & fextLocal) const; - - /// \brief Compute the force resulting from ground contact on a given frame. - /// - /// \param[in] system System for which to perform computation. - /// \param[in] frameIndex Index of the frame in contact. - /// - /// \returns Contact force, at parent joint, in the local frame. - void computeContactDynamicsAtFrame( - const System & system, - pinocchio::FrameIndex frameIndex, - std::shared_ptr & collisionConstraint, - pinocchio::Force & fextLocal) const; - - /// \brief Compute the ground reaction force for a given normal direction and depth. - pinocchio::Force computeContactDynamics(const Eigen::Vector3d & nGround, - double depth, - const Eigen::Vector3d & vContactInWorld) const; - - void computeCommand(System & system, - double t, - const Eigen::VectorXd & q, - const Eigen::VectorXd & v, - Eigen::VectorXd & command); - void computeInternalDynamics(const System & system, - SystemData & systemData, - double t, - const Eigen::VectorXd & q, - const Eigen::VectorXd & v, - Eigen::VectorXd & uInternal) const; - void computeCollisionForces(const System & system, - SystemData & systemData, - ForceVector & fext, - bool isStateUpToDate = false) const; - void computeExternalForces(const System & system, - SystemData & systemData, - double t, - const Eigen::VectorXd & q, - const Eigen::VectorXd & v, - ForceVector & fext); - void computeCouplingForces(double t, - const std::vector & qSplit, - const std::vector & vSplit); - void computeAllTerms(double t, - const std::vector & qSplit, - const std::vector & vSplit, - bool isStateUpToDate = false); - - /// \brief Compute system acceleration from current system state. - /// - /// \details This function performs forward dynamics computation, either with kinematic - /// constraints (using Lagrange multiplier for computing the forces) or - /// unconstrained (aba). - /// - /// \param[in] system System for which to compute the dynamics. - /// \param[in] q Joint position. - /// \param[in] v Joint velocity. - /// \param[in] u Joint effort. - /// \param[in] fext External forces applied on the system. - /// - /// \return System acceleration. - const Eigen::VectorXd & computeAcceleration(System & system, - SystemData & systemData, - const Eigen::VectorXd & q, - const Eigen::VectorXd & v, - const Eigen::VectorXd & u, - ForceVector & fext, - bool isStateUpToDate = false, - bool ignoreBounds = false); - - public: - std::shared_ptr getLog(); - - static LogData readLog(const std::string & filename, const std::string & format); - - void writeLog(const std::string & filename, const std::string & format); - - private: - template - class JointCollectionTpl, - typename ConfigVectorType, - typename TangentVectorType> - inline Scalar - kineticEnergy(const pinocchio::ModelTpl & model, - pinocchio::DataTpl & data, - const Eigen::MatrixBase & q, - const Eigen::MatrixBase & v, - bool update_kinematics); - template - class JointCollectionTpl, - typename ConfigVectorType, - typename TangentVectorType1, - typename TangentVectorType2, - typename ForceDerived> - inline const typename pinocchio::DataTpl:: - TangentVectorType & - rnea(const pinocchio::ModelTpl & model, - pinocchio::DataTpl & data, - const Eigen::MatrixBase & q, - const Eigen::MatrixBase & v, - const Eigen::MatrixBase & a, - const vector_aligned_t & fext); - template - class JointCollectionTpl, - typename ConfigVectorType, - typename TangentVectorType1, - typename TangentVectorType2, - typename ForceDerived> - inline const typename pinocchio::DataTpl:: - TangentVectorType & - aba(const pinocchio::ModelTpl & model, - pinocchio::DataTpl & data, - const Eigen::MatrixBase & q, - const Eigen::MatrixBase & v, - const Eigen::MatrixBase & tau, - const vector_aligned_t & fext); - - public: - std::unique_ptr engineOptions_{nullptr}; - std::vector systems_{}; - - protected: - bool isTelemetryConfigured_{false}; - bool isSimulationRunning_{false}; - GenericConfig engineOptionsGeneric_{}; - PCG32 generator_; - - private: - Timer timer_{}; - ContactModelType contactModel_{ContactModelType::UNSUPPORTED}; - std::unique_ptr telemetrySender_; - std::shared_ptr telemetryData_; - std::unique_ptr telemetryRecorder_; - std::unique_ptr stepper_{nullptr}; - double stepperUpdatePeriod_{INF}; - StepperState stepperState_{}; - vector_aligned_t systemDataVec_{}; - CouplingForceVector couplingForces_{}; - vector_aligned_t contactForcesPrev_{}; - vector_aligned_t fPrev_{}; - vector_aligned_t aPrev_{}; - std::vector energy_{}; - std::shared_ptr logData_{nullptr}; - }; -} - -#endif // JIMINY_ENGINE_MULTIROBOT_H diff --git a/core/include/jiminy/core/engine/system.h b/core/include/jiminy/core/engine/system.h deleted file mode 100644 index e750176cb..000000000 --- a/core/include/jiminy/core/engine/system.h +++ /dev/null @@ -1,199 +0,0 @@ - - - -#ifndef JIMINY_SYSTEM_H -#define JIMINY_SYSTEM_H - -#include - -#include "jiminy/core/fwd.h" -#include "jiminy/core/robot/model.h" - - -namespace jiminy -{ - class Robot; - class AbstractConstraintSolver; - class AbstractController; - class LockGuardLocal; - - // ******************************** External force functors ******************************** // - - using ProfileForceFunction = std::function; - - struct JIMINY_DLLAPI ProfileForce - { - public: - // FIXME: Designated aggregate initialization without constructors when moving to C++20 - explicit ProfileForce() = default; - explicit ProfileForce(const std::string & frameNameIn, - pinocchio::FrameIndex frameIndexIn, - double updatePeriodIn, - const ProfileForceFunction & forceFuncIn) noexcept; - - public: - std::string frameName{}; - pinocchio::FrameIndex frameIndex{0}; - double updatePeriod{0.0}; - pinocchio::Force force{pinocchio::Force::Zero()}; - ProfileForceFunction func{}; - }; - - struct JIMINY_DLLAPI ImpulseForce - { - public: - // FIXME: Designated aggregate initialization without constructors when moving to C++20 - explicit ImpulseForce() = default; - explicit ImpulseForce(const std::string & frameNameIn, - pinocchio::FrameIndex frameIndexIn, - double tIn, - double dtIn, - const pinocchio::Force & forceIn) noexcept; - - - public: - std::string frameName{}; - pinocchio::FrameIndex frameIndex{0}; - double t{0.0}; - double dt{0.0}; - pinocchio::Force force{}; - }; - - using CouplingForceFunction = - std::function; - - struct CouplingForce - { - public: - // FIXME: Designated aggregate initialization without constructors when moving to C++20 - explicit CouplingForce() = default; - explicit CouplingForce(const std::string & systemName1In, - std::ptrdiff_t systemIndex1In, - const std::string & systemName2In, - std::ptrdiff_t systemIndex2In, - const std::string & frameName1In, - pinocchio::FrameIndex frameIndex1In, - const std::string & frameName2In, - pinocchio::FrameIndex frameIndex2In, - const CouplingForceFunction & forceFunIn) noexcept; - - public: - std::string systemName1{}; - std::ptrdiff_t systemIndex1{-1}; - std::string systemName2{}; - std::ptrdiff_t systemIndex2{-1}; - std::string frameName1{}; - pinocchio::FrameIndex frameIndex1{0}; - std::string frameName2{}; - pinocchio::FrameIndex frameIndex2{0}; - CouplingForceFunction func{}; - }; - - using ProfileForceVector = std::vector; - using ImpulseForceVector = std::vector; - using CouplingForceVector = std::vector; - - // Early termination callback functor - using AbortSimulationFunction = std::function; - - struct JIMINY_DLLAPI System - { - std::string name{}; - std::shared_ptr robot{nullptr}; - std::shared_ptr controller{nullptr}; - AbortSimulationFunction callback{[](double /* t */, - const Eigen::VectorXd & /* q */, - const Eigen::VectorXd & /* v */) -> bool - { - return false; - }}; - }; - - // ************************************** System state ************************************* // - - struct JIMINY_DLLAPI SystemState - { - public: - void initialize(const Robot & robot); - bool getIsInitialized() const; - - void clear(); - - public: - Eigen::VectorXd q{}; - Eigen::VectorXd v{}; - Eigen::VectorXd a{}; - Eigen::VectorXd command{}; - Eigen::VectorXd u{}; - Eigen::VectorXd uMotor{}; - Eigen::VectorXd uInternal{}; - Eigen::VectorXd uCustom{}; - ForceVector fExternal{}; - - private: - bool isInitialized_{false}; - }; - - struct JIMINY_DLLAPI SystemData - { - public: - DISABLE_COPY(SystemData) - - /* Must move all definitions in source files to avoid compilation failure due to incomplete - destructor for objects managed by `unique_ptr` member variable with MSVC compiler. - See: https://stackoverflow.com/a/9954553 - https://developercommunity.visualstudio.com/t/unique-ptr-cant-delete-an-incomplete-type/1371585 - */ - explicit SystemData(); - explicit SystemData(SystemData &&); - SystemData & operator=(SystemData &&); - ~SystemData(); - - public: - std::unique_ptr robotLock{nullptr}; - - ProfileForceVector profileForces{}; - ImpulseForceVector impulseForces{}; - /// \brief Sorted list without repetitions of all the start/stop times of impulse forces. - std::set impulseForceBreakpoints{}; - /// \brief Time of the next breakpoint associated with the impulse forces. - std::set::const_iterator impulseForceBreakpointNextIt{}; - /// \brief Set of flags tracking whether each force is active. - /// - /// \details This flag is used to handle t-, t+ properly. Without it, it is impossible to - /// determine at time t if the force is active or not. - std::vector isImpulseForceActiveVec{}; - - uint32_t successiveSolveFailed{0}; - std::unique_ptr constraintSolver{nullptr}; - /// \brief Store copy of constraints register for fast access. - ConstraintTree constraints{}; - /// \brief Contact forces for each contact frames in local frame. - ForceVector contactFrameForces{}; - /// \brief Contact forces for each geometries of each collision bodies in local frame. - vector_aligned_t collisionBodiesForces{}; - /// \brief Jacobian of the joints in local frame. Used for computing `data.u`. - std::vector jointJacobians{}; - - std::vector logPositionFieldnames{}; - std::vector logVelocityFieldnames{}; - std::vector logAccelerationFieldnames{}; - std::vector logForceExternalFieldnames{}; - std::vector logCommandFieldnames{}; - std::vector logMotorEffortFieldnames{}; - std::string logEnergyFieldname{}; - - /// \brief Internal buffer with the state for the integration loop. - SystemState state{}; - /// \brief Internal state for the integration loop at the end of the previous iteration. - SystemState statePrev{}; - }; -} - -#endif // end of JIMINY_STEPPERS_H diff --git a/core/include/jiminy/core/hardware/abstract_sensor.hxx b/core/include/jiminy/core/hardware/abstract_sensor.hxx index d929bedd8..10dbbe4b4 100644 --- a/core/include/jiminy/core/hardware/abstract_sensor.hxx +++ b/core/include/jiminy/core/hardware/abstract_sensor.hxx @@ -289,7 +289,11 @@ namespace jiminy template void AbstractSensorTpl::interpolateData() { - assert(sharedStorage_->times_.size() > 0 && "No data to interpolatePositions."); + // Make sure that data is available + if (sharedStorage_->times_.empty()) + { + throw std::logic_error("No data to interpolate."); + } // Sample the delay uniformly const double delay = diff --git a/core/include/jiminy/core/hardware/fwd.h b/core/include/jiminy/core/hardware/fwd.h index f33fec6fe..d3b19c74b 100644 --- a/core/include/jiminy/core/hardware/fwd.h +++ b/core/include/jiminy/core/hardware/fwd.h @@ -67,8 +67,11 @@ namespace jiminy { if (sharedMeasurementsPtr_) { - assert((size() == static_cast(sharedMeasurementsPtr_->cols())) && - "Number of sensors inconsistent with shared measurements."); + if (size() != static_cast(sharedMeasurementsPtr_->cols())) + { + throw std::logic_error( + "Number of sensors inconsistent with shared measurements."); + } return *sharedMeasurementsPtr_; } else @@ -86,8 +89,11 @@ namespace jiminy // Set internal buffer by copying sensor data sequentially for (const auto & sensor : *this) { - assert(sensor.value.size() == dataSize && - "Cannot get all data at once for heterogeneous sensors."); + if (sensor.value.size() != dataSize) + { + throw std::logic_error( + "Cannot get all data at once for heterogeneous sensors."); + } data_.row(sensor.index) = sensor.value; } diff --git a/core/include/jiminy/core/robot/model.h b/core/include/jiminy/core/robot/model.h index 2bc0a5674..1e228df23 100644 --- a/core/include/jiminy/core/robot/model.h +++ b/core/include/jiminy/core/robot/model.h @@ -31,6 +31,9 @@ namespace jiminy USER = 3 }; + /* Note that following ordering plays a critical role as it determines in which order `foreach` + iterates over all the constraints. This has a directly effect on the solution found by 'PGS' + constraint solvers. */ inline constexpr std::array constraintNodeTypesAll{ConstraintNodeType::BOUNDS_JOINTS, ConstraintNodeType::CONTACT_FRAMES, ConstraintNodeType::COLLISION_BODIES, diff --git a/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h b/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h index 36af72d0d..2b535a4d8 100644 --- a/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h +++ b/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h @@ -30,7 +30,7 @@ #include "pinocchio/algorithm/jacobian.hpp" // `pinocchio::computeJointJacobians` #include "jiminy/core/fwd.h" -#include "jiminy/core/engine/engine_multi_robot.h" +#include "jiminy/core/engine/engine.h" namespace jiminy::pinocchio_overload @@ -494,10 +494,10 @@ namespace jiminy::pinocchio_overload } template - void computeJMinvJt(const pinocchio::Model & model, - pinocchio::Data & data, - const Eigen::MatrixBase & J, - bool updateDecomposition = true) + const Eigen::MatrixXd & computeJMinvJt(const pinocchio::Model & model, + pinocchio::Data & data, + const Eigen::MatrixBase & J, + bool updateDecomposition = true) { // Compute the Cholesky decomposition of mass matrix M if requested if (updateDecomposition) @@ -536,6 +536,8 @@ namespace jiminy::pinocchio_overload data.JMinvJt.resize(J.rows(), J.rows()); data.JMinvJt.triangularView().setZero(); data.JMinvJt.selfadjointView().rankUpdate(sDUiJt.transpose()); + + return data.JMinvJt; } template diff --git a/core/include/jiminy/core/robot/robot.h b/core/include/jiminy/core/robot/robot.h index 9390f46b9..d4d40579f 100644 --- a/core/include/jiminy/core/robot/robot.h +++ b/core/include/jiminy/core/robot/robot.h @@ -13,6 +13,7 @@ namespace jiminy class AbstractMotorBase; struct SensorSharedStorage; class AbstractSensorBase; + class AbstractController; class TelemetryData; class MutexLocal; class LockGuardLocal; @@ -28,7 +29,8 @@ namespace jiminy DISABLE_COPY(Robot) public: - explicit Robot() noexcept; + /// \param[in] name Name of the Robot. + explicit Robot(const std::string & name = "") noexcept; virtual ~Robot(); auto shared_from_this() { return shared_from(this); } @@ -42,6 +44,8 @@ namespace jiminy const std::vector & meshPackageDirs = {}, bool loadVisualMeshes = false); + const std::string & getName() const; + void attachMotor(std::shared_ptr motor); std::shared_ptr getMotor(const std::string & motorName); std::weak_ptr getMotor(const std::string & motorName) const; @@ -57,6 +61,10 @@ namespace jiminy void detachSensor(const std::string & sensorType, const std::string & sensorName); void detachSensors(const std::string & sensorType = {}); + void setController(const std::shared_ptr & controller); + std::shared_ptr getController(); + std::weak_ptr getController() const; + void computeMotorEfforts(double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, @@ -77,22 +85,14 @@ namespace jiminy void setOptions(const GenericConfig & robotOptions); GenericConfig getOptions() const noexcept; - void setMotorOptions(const std::string & motorName, const GenericConfig & motorOptions); + void setModelOptions(const GenericConfig & modelOptions); + GenericConfig getModelOptions() const; void setMotorsOptions(const GenericConfig & motorsOptions); - GenericConfig getMotorOptions(const std::string & motorName) const; GenericConfig getMotorsOptions() const; - void setSensorOptions(const std::string & sensorType, - const std::string & sensorName, - const GenericConfig & sensorOptions); - void setSensorsOptions(const std::string & sensorType, - const GenericConfig & sensorsOptions); void setSensorsOptions(const GenericConfig & sensorsOptions); - GenericConfig getSensorOptions(const std::string & sensorType, - const std::string & sensorName) const; - GenericConfig getSensorsOptions(const std::string & sensorType) const; GenericConfig getSensorsOptions() const; - void setModelOptions(const GenericConfig & modelOptions); - GenericConfig getModelOptions() const; + void setControllerOptions(const GenericConfig & controllerOptions); + GenericConfig getControllerOptions() const; void setTelemetryOptions(const GenericConfig & telemetryOptions); GenericConfig getTelemetryOptions() const; @@ -102,8 +102,7 @@ namespace jiminy /// \remarks Those methods are not intended to be called manually. The Engine is taking /// care of it. virtual void reset(const uniform_random_bit_generator_ref & g) override; - virtual void configureTelemetry(std::shared_ptr telemetryData, - const std::string & prefix = {}); + virtual void configureTelemetry(std::shared_ptr telemetryData); void updateTelemetry(); bool getIsTelemetryConfigured() const; @@ -133,9 +132,11 @@ namespace jiminy protected: bool isTelemetryConfigured_{false}; std::shared_ptr telemetryData_{nullptr}; + /// \brief Motors attached to the robot. MotorVector motors_{}; + /// \brief Sensors attached to the robot. SensorTree sensors_{}; - std::unordered_map sensorTelemetryOptions_{}; + std::unordered_map telemetryOptions_{}; /// \brief Name of the motors. std::vector motorNames_{}; /// \brief Name of the sensors. @@ -144,8 +145,12 @@ namespace jiminy std::vector logCommandFieldnames_{}; /// \brief Fieldnames of the motors effort. std::vector logMotorEffortFieldnames_{}; - /// \brief The number of motors. + /// \brief Number of motors. Eigen::Index nmotors_{0}; + /// \brief Controller of the robot. + std::shared_ptr controller_{nullptr}; + /// \brief Name of the robot. + std::string name_; private: std::unique_ptr mutexLocal_{std::make_unique()}; diff --git a/core/include/jiminy/core/solver/constraint_solvers.h b/core/include/jiminy/core/solver/constraint_solvers.h index 5af034557..ab9390a10 100644 --- a/core/include/jiminy/core/solver/constraint_solvers.h +++ b/core/include/jiminy/core/solver/constraint_solvers.h @@ -20,14 +20,6 @@ namespace jiminy struct ConstraintData { - public: - DISABLE_COPY(ConstraintData) - - public: - explicit ConstraintData() = default; - ConstraintData(ConstraintData && constraintData) = default; - - public: AbstractConstraintBase * constraint{nullptr}; Eigen::Index startIndex{-1}; bool isInactive{false}; @@ -42,7 +34,7 @@ namespace jiminy virtual ~AbstractConstraintSolver() = default; /// \brief Compute the solution of the Nonlinear Complementary Problem: - /// A x + b = w, + /// A x - b = w, /// s.t. (w[i] > 0 and x[i] = 0) or (w[i] = 0 and x[i] > 0 /// /// for non-linear boxed bounds lo(x) < x < hi(x): @@ -76,6 +68,7 @@ namespace jiminy private: void ProjectedGaussSeidelIter(const Eigen::MatrixXd & A, const Eigen::VectorXd::SegmentReturnType & b, + const double w, Eigen::VectorXd::SegmentReturnType & x); bool ProjectedGaussSeidelSolver(const Eigen::MatrixXd & A, const Eigen::VectorXd::SegmentReturnType & b, diff --git a/core/include/jiminy/core/stepper/runge_kutta_dopri_stepper.h b/core/include/jiminy/core/stepper/runge_kutta_dopri_stepper.h index 594371890..bd0e97ae3 100644 --- a/core/include/jiminy/core/stepper/runge_kutta_dopri_stepper.h +++ b/core/include/jiminy/core/stepper/runge_kutta_dopri_stepper.h @@ -38,7 +38,7 @@ namespace jiminy /// \details The larger the more conservative. More precisely, a small safety factor means /// that the step size will be increased less aggressively when the error is small /// and decreased more aggressively when the error is large. - inline constexpr double SAFETY = 0.9; + inline constexpr double SAFETY = 0.8; /// \brief Maximum acceptable error threshold below which step size is increased. inline constexpr double ERROR_THRESHOLD = 0.5; /// \brief Mininum allowed relative step decrease. diff --git a/core/include/jiminy/core/traits.h b/core/include/jiminy/core/traits.h index 98d1e3c2e..077e6eaf4 100644 --- a/core/include/jiminy/core/traits.h +++ b/core/include/jiminy/core/traits.h @@ -97,7 +97,7 @@ namespace jiminy namespace internal { - /// @sa For reference, see: + /// \sa For reference, see: /// https://stackoverflow.com/a/34672753/4820605 template class Base, typename Derived> struct IsBaseOfTemplateImpl diff --git a/core/include/jiminy/core/utilities/random.h b/core/include/jiminy/core/utilities/random.h index 5838ebba7..11894590f 100644 --- a/core/include/jiminy/core/utilities/random.h +++ b/core/include/jiminy/core/utilities/random.h @@ -16,16 +16,16 @@ namespace jiminy { // ***************************** Uniform random bit generators ***************************** // - /// @brief 32-bits Permuted Congruential Generator (PCG) random number generator. This + /// \brief 32-bits Permuted Congruential Generator (PCG) random number generator. This /// generator has excellent statistical quality while being much faster than Mersenne /// Twister (std::mt19937) and having acceptable period (2^62). /// - /// @details The PCG random number generation scheme has been developed by Melissa O'Neill. The + /// \details The PCG random number generation scheme has been developed by Melissa O'Neill. The /// technical details can be found in his origin paper, "PCG: A Family of Simple Fast /// Space-Efficient Statistically Good Algorithms for Random Number Generation.", /// 2014. For additional information, please visit: http://www.pcg-random.org /// - /// @warning This implementation has not been vectorized by leveraging SIMD instructions. As + /// \warning This implementation has not been vectorized by leveraging SIMD instructions. As /// such, all platforms are supported out-of-the-box and yield reproducible results, /// at the cost of running significantly slower to when it comes to sampling enough /// data at once (e.g. AVX512 operates on 16 floats at once). This use-case is largely @@ -33,7 +33,7 @@ namespace jiminy /// For completeness, here is a another fully vectorized implementation: /// https://github.com/lemire/simdpcg /// - /// @sa The proposed implementation is a minimal version of `pcg32_fast` released under the + /// \sa The proposed implementation is a minimal version of `pcg32_fast` released under the /// Apache License, Version 2.0: https://github.com/imneme/pcg-cpp class JIMINY_DLLAPI PCG32 { @@ -241,14 +241,14 @@ namespace jiminy uniform( Eigen::Index nrows, Eigen::Index ncols, Generator & g, float lo = 0.0F, float hi = 1.0F); - /// @details The original Ziggurat algorithm for single-precision floating-point scalars is + /// \details The original Ziggurat algorithm for single-precision floating-point scalars is /// used internally. This method is known to be about x4 times faster than the /// Box–Muller transform but significantly more complex to implement and more /// notably to vectorize using SIMD instructions. For details about these methods: /// https://en.wikipedia.org/wiki/Ziggurat_algorithm /// https://en.wikipedia.org/wiki/Box–Muller_transform /// - /// @warning This implementation has been optimized for sampling individual scalar here and + /// \warning This implementation has been optimized for sampling individual scalar here and /// there, rather than all at once in a vector. The proposed PCG32 random number /// generator is consistent with this design choice. Fully vectorized implementations /// of Ziggurat algorithm and Box-Muller transform that supports both x86 and @@ -257,7 +257,7 @@ namespace jiminy /// It you are looking for fully vectorized implementation of some other statistical /// distributions, have a look to this project: https://github.com/bab2min/EigenRand /// - /// @sa Based on the original implementation by Marsaglia and Tsang (JSS, 2000): + /// \sa Based on the original implementation by Marsaglia and Tsang (JSS, 2000): /// https://people.sc.fsu.edu/~jburkardt/cpp_src/ziggurat/ziggurat.html /// This implementation is known to fail some standard statistical tests as described /// by Doornik (2005). This is not an issue for most applications. For reference: @@ -415,11 +415,11 @@ namespace jiminy protected: virtual double grad(int32_t knot, double delta) const noexcept = 0; - /// @brief Improved Smoothstep function by Ken Perlin (aka Smootherstep). + /// \brief Improved Smoothstep function by Ken Perlin (aka Smootherstep). /// - /// @details It has zero 1st and 2nd-order derivatives at dt = 0.0, and 1.0. + /// \details It has zero 1st and 2nd-order derivatives at dt = 0.0, and 1.0. /// - /// @sa For reference, see: + /// \sa For reference, see: /// https://en.wikipedia.org/wiki/Smoothstep#Variations static double fade(double delta) noexcept; static double lerp(double ratio, double yLeft, double yRight) noexcept; diff --git a/core/src/constraints/joint_constraint.cc b/core/src/constraints/joint_constraint.cc index 8769e65fe..cdeffe604 100644 --- a/core/src/constraints/joint_constraint.cc +++ b/core/src/constraints/joint_constraint.cc @@ -152,7 +152,7 @@ namespace jiminy // Get the joint model const pinocchio::JointModel & jointModel = model->pinocchioModel_.joints[jointIndex_]; - // Add Baumgarte stabilization drift + // Add Baumgarte stabilization to drift const Eigen::VectorXd deltaPosition = difference(jointModel, configurationRef_, jointModel.jointConfigSelector(q)); drift_ = kp_ * deltaPosition + kd_ * jointModel.jointVelocitySelector(v); diff --git a/core/src/control/abstract_controller.cc b/core/src/control/abstract_controller.cc index 8daf5cb0e..14f5f930b 100644 --- a/core/src/control/abstract_controller.cc +++ b/core/src/control/abstract_controller.cc @@ -35,6 +35,22 @@ namespace jiminy THROW_ERROR(bad_control_flow, "Robot not initialized."); } + // Make sure that the controller is not already bound to another robot + if (isInitialized_) + { + auto robotOld = robot_.lock(); + if (robotOld && robotOld.get() != robot.get()) + { + auto controllerOld = robotOld->getController().lock(); + if (controllerOld && controllerOld.get() == this) + { + THROW_ERROR(bad_control_flow, + "Controller already bound to another robot. Please unbind it " + "first before re-initializing it."); + } + } + } + // Backup robot robot_ = robotIn; @@ -134,10 +150,10 @@ namespace jiminy { telemetrySender_->registerConstant(constantName, constantValue); } - for (const auto & [variableName, variableValuePtr] : variableRegistry_) + for (const auto & [variableNameIn, variableValuePtr] : variableRegistry_) { // FIXME: Remove explicit `name` capture when moving to C++20 - std::visit([&, &variableName = variableName](auto && arg) + std::visit([&, &variableName = variableNameIn](auto && arg) { telemetrySender_->registerVariable(variableName, arg); }, variableValuePtr); } diff --git a/core/src/engine/engine.cc b/core/src/engine/engine.cc index d89af4adc..2dc208215 100644 --- a/core/src/engine/engine.cc +++ b/core/src/engine/engine.cc @@ -1,276 +1,4280 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include "pinocchio/parsers/urdf.hpp" +#include "pinocchio/spatial/inertia.hpp" // `pinocchio::Inertia` +#include "pinocchio/spatial/force.hpp" // `pinocchio::Force` +#include "pinocchio/spatial/se3.hpp" // `pinocchio::SE3` +#include "pinocchio/spatial/explog.hpp" // `pinocchio::exp3`, `pinocchio::log3` +#include "pinocchio/spatial/explog-quaternion.hpp" // `pinocchio::quaternion::log3` +#include "pinocchio/multibody/visitor.hpp" // `pinocchio::fusion::JointUnaryVisitorBase` +#include "pinocchio/multibody/joint/joint-model-base.hpp" // `pinocchio::JointModelBase` +#include "pinocchio/algorithm/center-of-mass.hpp" // `pinocchio::getComFromCrba` +#include "pinocchio/algorithm/frames.hpp" // `pinocchio::getFrameVelocity` +#include "pinocchio/algorithm/jacobian.hpp" // `pinocchio::getJointJacobian` +#include "pinocchio/algorithm/energy.hpp" // `pinocchio::computePotentialEnergy` +#include "pinocchio/algorithm/joint-configuration.hpp" // `pinocchio::neutral`, `pinocchio::normalize` +#include "pinocchio/algorithm/geometry.hpp" // `pinocchio::computeCollisions` + +#include "H5Cpp.h" +#include "json/json.h" + +#include "jiminy/core/telemetry/fwd.h" +#include "jiminy/core/hardware/fwd.h" +#include "jiminy/core/utilities/helpers.h" +#include "jiminy/core/utilities/pinocchio.h" +#include "jiminy/core/utilities/json.h" +#include "jiminy/core/io/file_device.h" +#include "jiminy/core/io/serialization.h" +#include "jiminy/core/telemetry/telemetry_sender.h" +#include "jiminy/core/telemetry/telemetry_data.h" +#include "jiminy/core/telemetry/telemetry_recorder.h" +#include "jiminy/core/constraints/abstract_constraint.h" +#include "jiminy/core/constraints/joint_constraint.h" +#include "jiminy/core/constraints/frame_constraint.h" +#include "jiminy/core/hardware/abstract_motor.h" +#include "jiminy/core/hardware/abstract_sensor.h" #include "jiminy/core/robot/robot.h" +#include "jiminy/core/robot/pinocchio_overload_algorithms.h" #include "jiminy/core/control/abstract_controller.h" +#include "jiminy/core/solver/constraint_solvers.h" +#include "jiminy/core/stepper/abstract_stepper.h" +#include "jiminy/core/stepper/euler_explicit_stepper.h" +#include "jiminy/core/stepper/runge_kutta_dopri_stepper.h" +#include "jiminy/core/stepper/runge_kutta4_stepper.h" #include "jiminy/core/engine/engine.h" - namespace jiminy { - void Engine::initializeImpl(std::shared_ptr robot, - std::shared_ptr controller, - const AbortSimulationFunction & callback) - { - // Make sure the simulation is properly stopped - if (isSimulationRunning_) - { - stop(); - } - - // Remove the existing system if already initialized - if (isInitialized_) - { - EngineMultiRobot::removeSystem(""); - robot_ = nullptr; - controller_ = nullptr; - isInitialized_ = false; - } + inline constexpr uint32_t INIT_ITERATIONS{4U}; + inline constexpr uint32_t PGS_MAX_ITERATIONS{100U}; - // Add the system with empty name since it is irrelevant for a single robot engine - if (controller) - { - EngineMultiRobot::addSystem("", robot, controller, callback); - } - else - { - EngineMultiRobot::addSystem("", robot, callback); - } + // ******************************** External force functors ******************************** // - // Get some convenience proxies - robot_ = systems_.begin()->robot.get(); - controller_ = systems_.begin()->controller.get(); + ProfileForce::ProfileForce(const std::string & frameNameIn, + pinocchio::FrameIndex frameIndexIn, + double updatePeriodIn, + const ProfileForceFunction & forceFuncIn) noexcept : + frameName{frameNameIn}, + frameIndex{frameIndexIn}, + updatePeriod{updatePeriodIn}, + func{forceFuncIn} + { + } - // Set the initialization flag - isInitialized_ = true; + ImpulseForce::ImpulseForce(const std::string & frameNameIn, + pinocchio::FrameIndex frameIndexIn, + double tIn, + double dtIn, + const pinocchio::Force & forceIn) noexcept : + frameName{frameNameIn}, + frameIndex{frameIndexIn}, + t{tIn}, + dt{dtIn}, + force{forceIn} + { } - void Engine::initialize(std::shared_ptr robot, - std::shared_ptr controller, - const AbortSimulationFunction & callback) + CouplingForce::CouplingForce(const std::string & robotName1In, + std::ptrdiff_t robotIndex1In, + const std::string & robotName2In, + std::ptrdiff_t robotIndex2In, + const std::string & frameName1In, + pinocchio::FrameIndex frameIndex1In, + const std::string & frameName2In, + pinocchio::FrameIndex frameIndex2In, + const CouplingForceFunction & forceFuncIn) noexcept : + robotName1{robotName1In}, + robotIndex1{robotIndex1In}, + robotName2{robotName2In}, + robotIndex2{robotIndex2In}, + frameName1{frameName1In}, + frameIndex1{frameIndex1In}, + frameName2{frameName2In}, + frameIndex2{frameIndex2In}, + func{forceFuncIn} { - return initializeImpl(robot, controller, callback); } - void Engine::initialize(std::shared_ptr robot, const AbortSimulationFunction & callback) + // ************************************** System state ************************************* // + + void RobotState::initialize(const Robot & robot) { - return initializeImpl(robot, std::shared_ptr(), callback); + if (!robot.getIsInitialized()) + { + THROW_ERROR(bad_control_flow, "Robot not initialized."); + } + + Eigen::Index nv = robot.nv(); + std::size_t nMotors = robot.nmotors(); + std::size_t nJoints = robot.pinocchioModel_.njoints; + q = pinocchio::neutral(robot.pinocchioModel_); + v.setZero(nv); + a.setZero(nv); + command.setZero(nMotors); + u.setZero(nv); + uMotor.setZero(nMotors); + uInternal.setZero(nv); + uCustom.setZero(nv); + fExternal = ForceVector(nJoints, pinocchio::Force::Zero()); + isInitialized_ = true; } - void Engine::setController(std::shared_ptr controller) + bool RobotState::getIsInitialized() const { - return EngineMultiRobot::setController("", controller); + return isInitialized_; } - void Engine::addSystem(const std::string & /* systemName */, - std::shared_ptr /* robot */, - std::shared_ptr /* controller */) + void RobotState::clear() { - THROW_ERROR( - not_implemented_error, - "This method is not supported by this class. Please call `initialize` instead to set " - "the model, or use `EngineMultiRobot` class directly to simulate multiple robots " - "simultaneously."); + q.resize(0); + v.resize(0); + a.resize(0); + command.resize(0); + u.resize(0); + uMotor.resize(0); + uInternal.resize(0); + uCustom.resize(0); + fExternal.clear(); } - void Engine::removeSystem(const std::string & /* systemName */) + RobotData::RobotData() = default; + RobotData::RobotData(RobotData &&) = default; + RobotData & RobotData::operator=(RobotData &&) = default; + RobotData::~RobotData() = default; + + Engine::Engine() noexcept : + generator_{std::seed_seq{std::random_device{}()}}, + telemetrySender_{std::make_unique()}, + telemetryData_{std::make_shared()}, + telemetryRecorder_{std::make_unique()} { - THROW_ERROR( - not_implemented_error, - "This method is not supported by this class. Please call `initialize` instead to set " - "the model, or use `EngineMultiRobot` class directly to simulate multiple robots " - "simultaneously."); + // Initialize the configuration options to the default. + setOptions(getDefaultEngineOptions()); } - void singleToMultipleSystemsInitialData( - const Robot & robot, - bool isStateTheoretical, - const Eigen::VectorXd & qInit, - const Eigen::VectorXd & vInit, - const std::optional & aInit, - std::map & qInitList, - std::map & vInitList, - std::optional> & aInitList) + // ************************************ Engine *********************************** // + + // Cannot be default in the header since some types are incomplete at this point + Engine::~Engine() = default; + + void Engine::addRobot(std::shared_ptr robotIn) { - if (isStateTheoretical && robot.modelOptions_->dynamics.enableFlexibleModel) + // Make sure that no simulation is running + if (isSimulationRunning_) { - Eigen::VectorXd q0; - robot.getFlexiblePositionFromRigid(qInit, q0); - qInitList.emplace("", std::move(q0)); + THROW_ERROR(bad_control_flow, + "Simulation already running. Stop it before adding new robot."); } - else + + if (!robotIn) { - qInitList.emplace("", qInit); + THROW_ERROR(std::invalid_argument, "No robot specified."); } - if (isStateTheoretical && robot.modelOptions_->dynamics.enableFlexibleModel) + if (!robotIn->getIsInitialized()) { - Eigen::VectorXd v0; - robot.getFlexibleVelocityFromRigid(vInit, v0); - vInitList.emplace("", std::move(v0)); + THROW_ERROR(bad_control_flow, "Robot not initialized."); } - else + + /* All the robots must have a unique name. The latter will be used as prefix of telemetry + constants and variables in the log file. As an exception, the first robot added to the + engine is allowed to have no name. In such a case, no prefix will be added to telemetry + variables for this specific robot. This does not prevent adding other robots with + qualified names later on. This branching adds complexity internally, but simplifies + single-robot simulation for the end-user, which is by far the most common use-case. */ + const std::string & robotName = robotIn->getName(); + if (!robots_.empty() && robotName == "") { - vInitList.emplace("", vInit); + THROW_ERROR(std::invalid_argument, "All robots but the first one must have a name."); } - if (aInit) + // Check if a robot with the same name already exists + auto robotIt = std::find_if(robots_.begin(), + robots_.end(), + [&robotName](const auto & robot) + { return (robot->getName() == robotName); }); + if (robotIt != robots_.end()) + { + THROW_ERROR(std::invalid_argument, + "Robot with name '", + robotName, + "' already added to the engine."); + } + + robots_.push_back(std::move(robotIn)); + robotDataVec_.resize(robots_.size()); + } + + void Engine::removeRobot(const std::string & robotName) + { + // Make sure that no simulation is running + if (isSimulationRunning_) + { + THROW_ERROR(bad_control_flow, + "Simulation already running. Stop it before removing a robot."); + } + + /* Remove every coupling forces involving the robot. + Note that it is already checking that the robot exists. */ + removeCouplingForces(robotName); + + // Get robot index + std::ptrdiff_t robotIndex = getRobotIndex(robotName); + + // Update the robots' indices for the remaining coupling forces + for (auto & force : couplingForces_) { - aInitList.emplace(); - if (isStateTheoretical && robot.modelOptions_->dynamics.enableFlexibleModel) + if (force.robotIndex1 > robotIndex) { - Eigen::VectorXd a0; - robot.getFlexibleVelocityFromRigid(*aInit, a0); - aInitList->emplace("", std::move(a0)); + --force.robotIndex1; } - else + if (force.robotIndex2 > robotIndex) { - aInitList->emplace("", *aInit); + --force.robotIndex2; } } + + // Remove the robot from the list + robots_.erase(robots_.begin() + robotIndex); + robotDataVec_.erase(robotDataVec_.begin() + robotIndex); } - void Engine::start(const Eigen::VectorXd & qInit, - const Eigen::VectorXd & vInit, - const std::optional & aInit, - bool isStateTheoretical) + void Engine::registerCouplingForce(const std::string & robotName1, + const std::string & robotName2, + const std::string & frameName1, + const std::string & frameName2, + const CouplingForceFunction & forceFunc) { - if (!isInitialized_) + // Make sure that no simulation is running + if (isSimulationRunning_) { - THROW_ERROR(bad_control_flow, "Engine not initialized."); + THROW_ERROR(bad_control_flow, + "Simulation already running. Stop it before adding coupling forces."); } - std::map qInitList; - std::map vInitList; - std::optional> aInitList = std::nullopt; - singleToMultipleSystemsInitialData( - *robot_, isStateTheoretical, qInit, vInit, aInit, qInitList, vInitList, aInitList); + // Get robot and frame indices + const std::ptrdiff_t robotIndex1 = getRobotIndex(robotName1); + const std::ptrdiff_t robotIndex2 = getRobotIndex(robotName2); + const pinocchio::FrameIndex frameIndex1 = + getFrameIndex(robots_[robotIndex1]->pinocchioModel_, frameName1); + const pinocchio::FrameIndex frameIndex2 = + getFrameIndex(robots_[robotIndex2]->pinocchioModel_, frameName2); - EngineMultiRobot::start(qInitList, vInitList, aInitList); + // Make sure it is not coupling the exact same frame + if (robotIndex1 == robotIndex2 && frameIndex1 == frameIndex2) + { + THROW_ERROR(std::invalid_argument, + "A coupling force must involve two different frames."); + } + + couplingForces_.emplace_back(robotName1, + robotIndex1, + robotName2, + robotIndex2, + frameName1, + frameIndex1, + frameName2, + frameIndex2, + forceFunc); } - void Engine::simulate(double tEnd, - const Eigen::VectorXd & qInit, - const Eigen::VectorXd & vInit, - const std::optional & aInit, - bool isStateTheoretical) + void Engine::registerViscoelasticCouplingForce(const std::string & robotName1, + const std::string & robotName2, + const std::string & frameName1, + const std::string & frameName2, + const Vector6d & stiffness, + const Vector6d & damping, + double alpha) { - if (!isInitialized_) + // Make sure that the input arguments are valid + if ((stiffness.array() < 0.0).any() || (damping.array() < 0.0).any()) { - THROW_ERROR(bad_control_flow, "Engine not initialized."); + THROW_ERROR(std::invalid_argument, + "Stiffness and damping parameters must be positive."); } - std::map qInitList; - std::map vInitList; - std::optional> aInitList = std::nullopt; - singleToMultipleSystemsInitialData( - *robot_, isStateTheoretical, qInit, vInit, aInit, qInitList, vInitList, aInitList); + // Get robot and frame indices + Robot * robot1 = getRobot(robotName1).get(); + Robot * robot2 = getRobot(robotName2).get(); + pinocchio::FrameIndex frameIndex1 = getFrameIndex(robot1->pinocchioModel_, frameName1); + pinocchio::FrameIndex frameIndex2 = getFrameIndex(robot2->pinocchioModel_, frameName2); + + // Allocate memory + double angle{0.0}; + Eigen::Matrix3d rot12{}, rotJLog12{}, rotJExp12{}, rotRef12{}, omega{}; + Eigen::Vector3d rotLog12{}, pos12{}, posLocal12{}, fLin{}, fAng{}; + + auto forceFunc = [=](double /* t */, + const Eigen::VectorXd & /* q1 */, + const Eigen::VectorXd & /* v1 */, + const Eigen::VectorXd & /* q2 */, + const Eigen::VectorXd & /* v2 */) mutable -> pinocchio::Force + { + /* One must keep track of robot pointers and frames indices internally and update + them at reset since the robots may have changed between simulations. Note that + `isSimulationRunning_` is false when called for the first time in `start` method + before locking the robot, so it is the right time to update those proxies. */ + if (!isSimulationRunning_) + { + robot1 = getRobot(robotName1).get(); + robot2 = getRobot(robotName2).get(); + frameIndex1 = getFrameIndex(robot1->pinocchioModel_, frameName1); + frameIndex2 = getFrameIndex(robot2->pinocchioModel_, frameName2); + } + + // Get the frames positions and velocities in world + const pinocchio::SE3 & oMf1{robot1->pinocchioData_.oMf[frameIndex1]}; + const pinocchio::SE3 & oMf2{robot2->pinocchioData_.oMf[frameIndex2]}; + const pinocchio::Motion oVf1{getFrameVelocity(robot1->pinocchioModel_, + robot1->pinocchioData_, + frameIndex1, + pinocchio::LOCAL_WORLD_ALIGNED)}; + const pinocchio::Motion oVf2{getFrameVelocity(robot2->pinocchioModel_, + robot2->pinocchioData_, + frameIndex2, + pinocchio::LOCAL_WORLD_ALIGNED)}; + + // Compute intermediary quantities + rot12.noalias() = oMf1.rotation().transpose() * oMf2.rotation(); + rotLog12 = pinocchio::log3(rot12, angle); + if (angle > 0.95 * M_PI) + { + THROW_ERROR(std::runtime_error, + "Relative angle between reference frames of viscoelastic " + "coupling must be smaller than 0.95 * pi."); + } + pinocchio::Jlog3(angle, rotLog12, rotJLog12); + fAng = stiffness.tail<3>().array() * rotLog12.array(); + rotLog12 *= alpha; + pinocchio::Jexp3(rotLog12, rotJExp12); + rotRef12.noalias() = oMf1.rotation() * pinocchio::exp3(rotLog12); + pos12 = oMf2.translation() - oMf1.translation(); + posLocal12.noalias() = rotRef12.transpose() * pos12; + fLin = stiffness.head<3>().array() * posLocal12.array(); + omega.noalias() = alpha * rotJExp12 * rotJLog12; + + /* Compute the relative velocity. The application point is the "linear" + interpolation between the frames placement with alpha ratio. */ + const pinocchio::Motion oVf12{oVf2 - oVf1}; + pinocchio::Motion velLocal12{ + rotRef12.transpose() * + (oVf12.linear() + pos12.cross(oVf2.angular() - alpha * oVf12.angular())), + rotRef12.transpose() * oVf12.angular()}; + + // Compute the coupling force acting on frame 2 + pinocchio::Force f{}; + f.linear() = damping.head<3>().array() * velLocal12.linear().array(); + f.angular() = (1.0 - alpha) * f.linear().cross(posLocal12); + f.angular().array() += damping.tail<3>().array() * velLocal12.angular().array(); + f.linear() += fLin; + f.linear() = rotRef12 * f.linear(); + f.angular() = rotRef12 * f.angular(); + f.angular() -= oMf2.rotation() * omega.colwise().cross(posLocal12).transpose() * fLin; + f.angular() += oMf1.rotation() * rotJLog12 * fAng; + + // Deduce the force acting on frame 1 from action-reaction law + f.angular() += pos12.cross(f.linear()); + + return f; + }; - EngineMultiRobot::simulate(tEnd, qInitList, vInitList, aInitList); + registerCouplingForce(robotName1, robotName2, frameName1, frameName2, forceFunc); } - void Engine::registerImpulseForce( - const std::string & frameName, double t, double dt, const pinocchio::Force & force) + void Engine::registerViscoelasticCouplingForce(const std::string & robotName, + const std::string & frameName1, + const std::string & frameName2, + const Vector6d & stiffness, + const Vector6d & damping, + double alpha) { - return EngineMultiRobot::registerImpulseForce("", frameName, t, dt, force); + return registerViscoelasticCouplingForce( + robotName, robotName, frameName1, frameName2, stiffness, damping, alpha); } - void Engine::registerProfileForce( - const std::string & frameName, const ProfileForceFunction & forceFunc, double updatePeriod) + void Engine::registerViscoelasticDirectionalCouplingForce(const std::string & robotName1, + const std::string & robotName2, + const std::string & frameName1, + const std::string & frameName2, + double stiffness, + double damping, + double restLength) { - return EngineMultiRobot::registerProfileForce("", frameName, forceFunc, updatePeriod); + // Make sure that the input arguments are valid + if (stiffness < 0.0 || damping < 0.0) + { + THROW_ERROR(std::invalid_argument, + "The stiffness and damping parameters must be positive."); + } + + // Get robot and frame indices + Robot * robot1 = getRobot(robotName1).get(); + Robot * robot2 = getRobot(robotName2).get(); + pinocchio::FrameIndex frameIndex1 = getFrameIndex(robot1->pinocchioModel_, frameName1); + pinocchio::FrameIndex frameIndex2 = getFrameIndex(robot2->pinocchioModel_, frameName2); + + auto forceFunc = [=](double /* t */, + const Eigen::VectorXd & /* q1 */, + const Eigen::VectorXd & /* v1 */, + const Eigen::VectorXd & /* q2 */, + const Eigen::VectorXd & /* v2 */) mutable -> pinocchio::Force + { + /* One must keep track of robot pointers and frames indices internally and update + them at reset since the robots may have changed between simulations. Note that + `isSimulationRunning_` is false when called for the first time in `start` method + before locking the robot, so it is the right time to update those proxies. */ + if (!isSimulationRunning_) + { + robot1 = getRobot(robotName1).get(); + robot2 = getRobot(robotName2).get(); + frameIndex1 = getFrameIndex(robot1->pinocchioModel_, frameName1); + frameIndex2 = getFrameIndex(robot2->pinocchioModel_, frameName2); + } + + // Get the frames positions and velocities in world + const pinocchio::SE3 & oMf1{robot1->pinocchioData_.oMf[frameIndex1]}; + const pinocchio::SE3 & oMf2{robot2->pinocchioData_.oMf[frameIndex2]}; + const pinocchio::Motion oVf1{getFrameVelocity(robot1->pinocchioModel_, + robot1->pinocchioData_, + frameIndex1, + pinocchio::LOCAL_WORLD_ALIGNED)}; + const pinocchio::Motion oVf2{getFrameVelocity(robot2->pinocchioModel_, + robot2->pinocchioData_, + frameIndex2, + pinocchio::LOCAL_WORLD_ALIGNED)}; + + // Compute the linear force coupling them + Eigen::Vector3d dir12{oMf2.translation() - oMf1.translation()}; + const double length{dir12.norm()}; + auto vel12 = oVf2.linear() - oVf1.linear(); + if (length > EPS) + { + dir12 /= length; + auto vel12Proj = vel12.dot(dir12); + return {(stiffness * (length - restLength) + damping * vel12Proj) * dir12, + Eigen::Vector3d::Zero()}; + } + /* The direction between frames is ill-defined, so applying force in the direction + of the linear velocity instead. */ + return {damping * vel12, Eigen::Vector3d::Zero()}; + }; + + registerCouplingForce(robotName1, robotName2, frameName1, frameName2, forceFunc); } - void Engine::removeImpulseForces() + void Engine::registerViscoelasticDirectionalCouplingForce(const std::string & robotName, + const std::string & frameName1, + const std::string & frameName2, + double stiffness, + double damping, + double restLength) { - return EngineMultiRobot::removeImpulseForces(""); + return registerViscoelasticDirectionalCouplingForce( + robotName, robotName, frameName1, frameName2, stiffness, damping, restLength); } - void Engine::removeProfileForces() + void Engine::removeCouplingForces(const std::string & robotName1, + const std::string & robotName2) { - return EngineMultiRobot::removeProfileForces(""); + // Make sure that no simulation is running + if (isSimulationRunning_) + { + THROW_ERROR(bad_control_flow, + "Simulation already running. Stop it before removing coupling forces."); + } + + // Make sure that the robots exist + getRobot(robotName1); + getRobot(robotName2); + + // Remove corresponding coupling forces if any + couplingForces_.erase(std::remove_if(couplingForces_.begin(), + couplingForces_.end(), + [&robotName1, &robotName2](const auto & force) { + return (force.robotName1 == robotName1 && + force.robotName2 == robotName2); + }), + couplingForces_.end()); } - const ImpulseForceVector & Engine::getImpulseForces() const + void Engine::removeCouplingForces(const std::string & robotName) { - return EngineMultiRobot::getImpulseForces(""); + // Make sure that no simulation is running + if (isSimulationRunning_) + { + THROW_ERROR(bad_control_flow, + "Simulation already running. Stop it before removing coupling forces."); + } + + // Make sure that the robot exists + getRobot(robotName); + + // Remove corresponding coupling forces if any + couplingForces_.erase(std::remove_if(couplingForces_.begin(), + couplingForces_.end(), + [&robotName](const auto & force) { + return (force.robotName1 == robotName || + force.robotName2 == robotName); + }), + couplingForces_.end()); } - const ProfileForceVector & Engine::getProfileForces() const + void Engine::removeCouplingForces() { - return EngineMultiRobot::getProfileForces(""); + // Make sure that no simulation is running + if (isSimulationRunning_) + { + THROW_ERROR(bad_control_flow, + "Simulation already running. Stop it before removing coupling forces."); + } + + couplingForces_.clear(); } - void Engine::registerCouplingForce(const std::string & frameName1, - const std::string & frameName2, - const ProfileForceFunction & forceFunc) + const CouplingForceVector & Engine::getCouplingForces() const { - auto couplingForceFun = [forceFunc](double t, - const Eigen::VectorXd & q1, - const Eigen::VectorXd & v1, - const Eigen::VectorXd & /* q2 */, - const Eigen::VectorXd & /* v2 */) - { - return forceFunc(t, q1, v1); - }; - return EngineMultiRobot::registerCouplingForce( - "", "", frameName1, frameName2, couplingForceFun); + return couplingForces_; } - void Engine::registerViscoelasticCouplingForce(const std::string & frameName1, - const std::string & frameName2, - const Vector6d & stiffness, - const Vector6d & damping, - double alpha) + void Engine::removeAllForces() { - return EngineMultiRobot::registerViscoelasticCouplingForce( - "", "", frameName1, frameName2, stiffness, damping, alpha); + removeCouplingForces(); + removeImpulseForces(); + removeProfileForces(); } - void Engine::registerViscoelasticDirectionalCouplingForce(const std::string & frameName1, - const std::string & frameName2, - double stiffness, - double damping, - double restLength) + void Engine::configureTelemetry() { - return EngineMultiRobot::registerViscoelasticDirectionalCouplingForce( - "", "", frameName1, frameName2, stiffness, damping, restLength); + if (robots_.empty()) + { + THROW_ERROR(bad_control_flow, "No robot added to the engine."); + } + + if (!isTelemetryConfigured_) + { + // Initialize the engine-specific telemetry sender + telemetrySender_->configure(telemetryData_, ENGINE_TELEMETRY_NAMESPACE); + + auto robotIt = robots_.begin(); + auto robotDataIt = robotDataVec_.begin(); + auto energyIt = energy_.begin(); + for (; robotIt != robots_.end(); ++robotIt, ++robotDataIt, ++energyIt) + { + // Generate the log fieldnames + robotDataIt->logPositionFieldnames = + addCircumfix((*robotIt)->getLogPositionFieldnames(), + (*robotIt)->getName(), + {}, + TELEMETRY_FIELDNAME_DELIMITER); + robotDataIt->logVelocityFieldnames = + addCircumfix((*robotIt)->getLogVelocityFieldnames(), + (*robotIt)->getName(), + {}, + TELEMETRY_FIELDNAME_DELIMITER); + robotDataIt->logAccelerationFieldnames = + addCircumfix((*robotIt)->getLogAccelerationFieldnames(), + (*robotIt)->getName(), + {}, + TELEMETRY_FIELDNAME_DELIMITER); + robotDataIt->logForceExternalFieldnames = + addCircumfix((*robotIt)->getLogForceExternalFieldnames(), + (*robotIt)->getName(), + {}, + TELEMETRY_FIELDNAME_DELIMITER); + robotDataIt->logCommandFieldnames = + addCircumfix((*robotIt)->getLogCommandFieldnames(), + (*robotIt)->getName(), + {}, + TELEMETRY_FIELDNAME_DELIMITER); + robotDataIt->logMotorEffortFieldnames = + addCircumfix((*robotIt)->getLogMotorEffortFieldnames(), + (*robotIt)->getName(), + {}, + TELEMETRY_FIELDNAME_DELIMITER); + robotDataIt->logEnergyFieldname = addCircumfix( + "energy", (*robotIt)->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER); + + // Register variables to the telemetry senders + if (engineOptions_->telemetry.enableConfiguration) + { + telemetrySender_->registerVariable(robotDataIt->logPositionFieldnames, + robotDataIt->state.q); + } + if (engineOptions_->telemetry.enableVelocity) + { + telemetrySender_->registerVariable(robotDataIt->logVelocityFieldnames, + robotDataIt->state.v); + } + if (engineOptions_->telemetry.enableAcceleration) + { + telemetrySender_->registerVariable(robotDataIt->logAccelerationFieldnames, + robotDataIt->state.a); + } + if (engineOptions_->telemetry.enableForceExternal) + { + for (std::size_t i = 1; i < robotDataIt->state.fExternal.size(); ++i) + { + const auto & fext = robotDataIt->state.fExternal[i].toVector(); + for (uint8_t j = 0; j < 6U; ++j) + { + telemetrySender_->registerVariable( + robotDataIt->logForceExternalFieldnames[(i - 1) * 6U + j], + &fext[j]); + } + } + } + if (engineOptions_->telemetry.enableCommand) + { + telemetrySender_->registerVariable(robotDataIt->logCommandFieldnames, + robotDataIt->state.command); + } + if (engineOptions_->telemetry.enableMotorEffort) + { + telemetrySender_->registerVariable(robotDataIt->logMotorEffortFieldnames, + robotDataIt->state.uMotor); + } + if (engineOptions_->telemetry.enableEnergy) + { + telemetrySender_->registerVariable(robotDataIt->logEnergyFieldname, + &(*energyIt)); + } + (*robotIt)->configureTelemetry(telemetryData_); + } + } + + isTelemetryConfigured_ = true; } - void Engine::removeCouplingForces() + void Engine::updateTelemetry() { - return EngineMultiRobot::removeCouplingForces(""); + // Compute the total energy of the robot + auto robotIt = robots_.begin(); + auto energyIt = energy_.begin(); + for (; robotIt != robots_.end(); ++robotIt, ++energyIt) + { + *energyIt = (*robotIt)->pinocchioData_.kinetic_energy + + (*robotIt)->pinocchioData_.potential_energy; + } + + // Update robot-specific telemetry variables + for (auto & robot : robots_) + { + robot->updateTelemetry(); + } + + // Update engine-specific telemetry variables + telemetrySender_->updateValues(); + + // Flush the telemetry internal state + telemetryRecorder_->flushSnapshot(stepperState_.t); } - bool Engine::getIsInitialized() const + void Engine::reset(bool resetRandomNumbers, bool removeAllForce) { - return isInitialized_; + // Make sure the simulation is properly stopped + if (isSimulationRunning_) + { + stop(); + } + + // Clear log data buffer + logData_.reset(); + + // Reset the dynamic force register if requested + if (removeAllForce) + { + for (auto & robotData : robotDataVec_) + { + robotData.impulseForces.clear(); + robotData.impulseForceBreakpoints.clear(); + robotData.isImpulseForceActiveVec.clear(); + robotData.profileForces.clear(); + } + // FIXME: replaced `std::get` by placeholder `_` when moving to C++26 (P2169R4) + stepperUpdatePeriod_ = + std::get<1>(isGcdIncluded(engineOptions_->stepper.sensorsUpdatePeriod, + engineOptions_->stepper.controllerUpdatePeriod)); + } + + // Reset the random number generators + if (resetRandomNumbers) + { + VectorX seedSeq = engineOptions_->stepper.randomSeedSeq; + generator_.seed(std::seed_seq(seedSeq.data(), seedSeq.data() + seedSeq.size())); + } + + // Reset the internal state of the robot + for (auto & robot : robots_) + { + robot->reset(generator_); + } + + // Clear robot state buffers, since the robot kinematic may change + for (auto & robotData : robotDataVec_) + { + robotData.state.clear(); + robotData.statePrev.clear(); + } + + isTelemetryConfigured_ = false; } - System & Engine::getSystem() + struct ForwardKinematicsAccelerationStep : + public pinocchio::fusion::JointUnaryVisitorBase + { + typedef boost::fusion::vector ArgsType; + + template + static void algo(const pinocchio::JointModelBase & jmodel, + pinocchio::JointDataBase & jdata, + pinocchio::Data & data, + const Eigen::VectorXd & a) + { + pinocchio::JointIndex jointIndex = jmodel.id(); + data.a[jointIndex] = jdata.c() + data.v[jointIndex].cross(jdata.v()); + data.a[jointIndex] += jdata.S() * jmodel.jointVelocitySelector(a); + } + }; + + /// \details This method is optimized to avoid redundant computations. + /// + /// \see See `pinocchio::computeAllTerms` for reference: + /// 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 + void computeExtraTerms( + std::shared_ptr & robot, const RobotData & robotData, ForceVector & fExt) { - if (!isInitialized_) + const pinocchio::Model & model = robot->pinocchioModel_; + pinocchio::Data & data = robot->pinocchioData_; + + // Compute the potential and kinematic energy of the robot + pinocchio_overload::computeKineticEnergy( + model, data, robotData.state.q, robotData.state.v, false); + pinocchio::computePotentialEnergy(model, data); + + /* Update manually the subtree (apparent) inertia, since it is only computed by crba, which + is doing more computation than necessary. It will be used here for computing the + centroidal kinematics, and used later for joint bounds dynamics. Note that, by doing all + the computations here instead of 'computeForwardKinematics', we are doing the assumption + that it is varying slowly enough to consider it constant during one integration step. */ + if (!robot->hasConstraints()) + { + for (int i = 1; i < model.njoints; ++i) + { + data.Ycrb[i] = model.inertias[i]; + } + for (int jointIndex = model.njoints - 1; jointIndex > 0; --jointIndex) + { + const pinocchio::JointIndex parentJointIndex = model.parents[jointIndex]; + if (parentJointIndex > 0) + { + data.Ycrb[parentJointIndex] += + data.liMi[jointIndex].act(data.Ycrb[jointIndex]); + } + } + } + + /* Neither 'aba' nor 'forwardDynamics' are computing simultaneously the actual joint + accelerations, joint forces and body forces, so it must be done separately: + - 1st step: computing the accelerations based on ForwardKinematic algorithm + - 2nd step: computing the forces based on RNEA algorithm */ + + /* Compute the true joint acceleration and the one due to the lone gravity field, then + the spatial momenta and the total internal and external forces acting on each body. */ + data.h[0].setZero(); + fExt[0].setZero(); + data.f[0].setZero(); + data.a[0].setZero(); + data.a_gf[0] = -model.gravity; + for (int jointIndex = 1; jointIndex < model.njoints; ++jointIndex) + { + ForwardKinematicsAccelerationStep::run( + model.joints[jointIndex], + data.joints[jointIndex], + typename ForwardKinematicsAccelerationStep::ArgsType(data, robotData.state.a)); + + const pinocchio::JointIndex parentJointIndex = model.parents[jointIndex]; + data.a_gf[jointIndex] = data.a[jointIndex]; + data.a[jointIndex] += data.liMi[jointIndex].actInv(data.a[parentJointIndex]); + data.a_gf[jointIndex] += data.liMi[jointIndex].actInv(data.a_gf[parentJointIndex]); + + model.inertias[jointIndex].__mult__(data.v[jointIndex], data.h[jointIndex]); + + model.inertias[jointIndex].__mult__(data.a[jointIndex], fExt[jointIndex]); + data.f[jointIndex] = data.v[jointIndex].cross(data.h[jointIndex]); + fExt[jointIndex] += data.f[jointIndex]; + data.f[jointIndex] += model.inertias[jointIndex] * data.a_gf[jointIndex]; + data.f[jointIndex] -= robotData.state.fExternal[jointIndex]; + } + for (int jointIndex = model.njoints - 1; jointIndex > 0; --jointIndex) + { + const pinocchio::JointIndex parentJointIndex = model.parents[jointIndex]; + fExt[parentJointIndex] += data.liMi[jointIndex].act(fExt[jointIndex]); + data.h[parentJointIndex] += data.liMi[jointIndex].act(data.h[jointIndex]); + if (parentJointIndex > 0) + { + data.f[parentJointIndex] += data.liMi[jointIndex].act(data.f[jointIndex]); + } + } + + // Compute the position and velocity of the center of mass of each subtree + for (int jointIndex = 0; jointIndex < model.njoints; ++jointIndex) { - THROW_ERROR(bad_control_flow, "Engine not initialized."); + if (jointIndex > 0) + { + data.com[jointIndex] = data.Ycrb[jointIndex].lever(); + } + data.vcom[jointIndex].noalias() = data.h[jointIndex].linear() / data.mass[jointIndex]; } + data.com[0] = data.liMi[1].act(data.com[1]); + + // Compute centroidal dynamics and its derivative + data.hg = data.h[0]; + data.hg.angular() += data.hg.linear().cross(data.com[0]); + data.dhg = fExt[0]; + data.dhg.angular() += data.dhg.linear().cross(data.com[0]); + } - return *systems_.begin(); + void computeAllExtraTerms(std::vector> & robots, + const vector_aligned_t & robotDataVec, + vector_aligned_t & f) + { + auto robotIt = robots.begin(); + auto robotDataIt = robotDataVec.begin(); + auto fIt = f.begin(); + for (; robotIt != robots.end(); ++robotIt, ++robotDataIt, ++fIt) + { + computeExtraTerms(*robotIt, *robotDataIt, *fIt); + } } - std::shared_ptr Engine::getRobot() + void syncAccelerationsAndForces(const std::shared_ptr & robot, + ForceVector & contactForces, + ForceVector & f, + MotionVector & a) { - return getSystem().robot; + for (std::size_t i = 0; i < robot->getContactFrameNames().size(); ++i) + { + contactForces[i] = robot->contactForces_[i]; + } + + for (int i = 0; i < robot->pinocchioModel_.njoints; ++i) + { + f[i] = robot->pinocchioData_.f[i]; + a[i] = robot->pinocchioData_.a[i]; + } } - std::shared_ptr Engine::getController() + void syncAllAccelerationsAndForces(const std::vector> & robots, + vector_aligned_t & contactForces, + vector_aligned_t & f, + vector_aligned_t & a) { - return getSystem().controller; + std::vector>::const_iterator robotIt = robots.begin(); + auto contactForceIt = contactForces.begin(); + auto fPrevIt = f.begin(); + auto aPrevIt = a.begin(); + for (; robotIt != robots.end(); ++robotIt, ++contactForceIt, ++fPrevIt, ++aPrevIt) + { + syncAccelerationsAndForces(*robotIt, *contactForceIt, *fPrevIt, *aPrevIt); + } } - const SystemState & Engine::getSystemState() const + void Engine::start(const std::map & qInit, + const std::map & vInit, + const std::optional> & aInit) { - if (!isInitialized_) + // Make sure that no simulation is running + if (isSimulationRunning_) + { + THROW_ERROR(bad_control_flow, + "Simulation already running. Please stop it before starting a new one."); + } + + if (robots_.empty()) + { + THROW_ERROR(bad_control_flow, + "No robot to simulate. Please add one before starting a simulation."); + } + + const std::size_t nRobots = robots_.size(); + if (qInit.size() != nRobots || vInit.size() != nRobots) + { + THROW_ERROR( + std::invalid_argument, + "Mismatching between number of robots and initial configurations or velocities."); + } + + // Check the dimension of the initial state associated with every robot and order them + std::vector qSplit; + std::vector vSplit; + qSplit.reserve(nRobots); + vSplit.reserve(nRobots); + for (const auto & robot : robots_) + { + const std::string & robotName = robot->getName(); + + auto qInitIt = qInit.find(robotName); + auto vInitIt = vInit.find(robotName); + if (qInitIt == qInit.end() || vInitIt == vInit.end()) + { + THROW_ERROR(std::invalid_argument, + "Robot '", + robotName, + "'does not have an initial configuration or velocity."); + } + + const Eigen::VectorXd & q = qInitIt->second; + const Eigen::VectorXd & v = vInitIt->second; + if (q.rows() != robot->nq() || v.rows() != robot->nv()) + { + THROW_ERROR(std::invalid_argument, + "The dimension of the initial configuration or velocity is " + "inconsistent with model size for robot '", + robotName, + "'."); + } + + // Make sure the initial state is normalized + const bool isValid = + isPositionValid(robot->pinocchioModel_, q, std::numeric_limits::epsilon()); + if (!isValid) + { + THROW_ERROR(std::invalid_argument, + "Initial configuration not consistent with model for robot '", + robotName, + "'."); + } + + /* Check that the initial configuration is not out-of-bounds if appropriate. + Note that EPS allows to be very slightly out-of-bounds, which may occurs because of + rounding errors. */ + if ((robot->modelOptions_->joints.enablePositionLimit && + (contactModel_ == ContactModelType::CONSTRAINT) && + ((EPS < q.array() - robot->getPositionLimitMax().array()).any() || + (EPS < robot->getPositionLimitMin().array() - q.array()).any()))) + { + THROW_ERROR(std::invalid_argument, + "Initial configuration out-of-bounds for robot '", + robotName, + "'."); + } + + // Check that the initial velocity is not out-of-bounds + if ((robot->modelOptions_->joints.enableVelocityLimit && + (robot->getVelocityLimit().array() < v.array().abs()).any())) + { + THROW_ERROR(std::invalid_argument, + "Initial velocity out-of-bounds for robot '", + robotName, + "'."); + } + + /* Make sure the configuration is normalized (as double), since normalization is + checked using float accuracy rather than double to circumvent float/double casting + than may occurs because of Python bindings. */ + Eigen::VectorXd qNormalized = q; + pinocchio::normalize(robot->pinocchioModel_, qNormalized); + + qSplit.emplace_back(qNormalized); + vSplit.emplace_back(v); + } + + std::vector aSplit; + aSplit.reserve(nRobots); + if (aInit) + { + // Check the dimension of the initial acceleration associated with every robot + if (aInit->size() != nRobots) + { + THROW_ERROR(std::invalid_argument, + "If specified, the number of initial accelerations must match the " + "number of robots."); + } + + for (const auto & robot : robots_) + { + auto aInitIt = aInit->find(robot->getName()); + if (aInitIt == aInit->end()) + { + THROW_ERROR(std::invalid_argument, + "Robot '", + robot->getName(), + "'does not have an initial acceleration."); + } + + const Eigen::VectorXd & a = aInitIt->second; + if (a.rows() != robot->nv()) + { + THROW_ERROR( + std::invalid_argument, + "Dimension of initial acceleration inconsistent with model for robot '", + robot->getName(), + "'."); + } + + aSplit.emplace_back(a); + } + } + else + { + // Zero acceleration by default + std::transform(vSplit.begin(), + vSplit.end(), + std::back_inserter(aSplit), + [](const auto & v) -> Eigen::VectorXd + { return Eigen::VectorXd::Zero(v.size()); }); + } + + for (auto & robot : robots_) + { + for (const auto & sensorGroupItem : robot->getSensors()) + { + for (const auto & sensor : sensorGroupItem.second) + { + if (!sensor->getIsInitialized()) + { + THROW_ERROR(bad_control_flow, + "At least a sensor of a robot is not initialized."); + } + } + } + + for (const auto & motor : robot->getMotors()) + { + if (!motor->getIsInitialized()) + { + THROW_ERROR(bad_control_flow, + "At least a motor of a robot is not initialized."); + } + } + } + + /* Call reset if the internal state of the engine is not clean. Not doing it robotatically + gives the opportunity to the user to customize the robot by resetting first the engine + manually and then to alter the robot before starting a simulation, e.g. to change the + inertia of a specific body. */ + if (isTelemetryConfigured_) + { + reset(false, false); + } + + // Reset the internal state of the robot + auto robotIt = robots_.begin(); + auto robotDataIt = robotDataVec_.begin(); + for (; robotIt != robots_.end(); ++robotIt, ++robotDataIt) + { + // Propagate the user-defined gravity at robot level + (*robotIt)->pinocchioModelOrig_.gravity = engineOptions_->world.gravity; + (*robotIt)->pinocchioModel_.gravity = engineOptions_->world.gravity; + + /* Reinitialize the robot state buffers, since the robot kinematic may have changed. + For example, it may happens if one activates or deactivates the flexibility between + two successive simulations. */ + robotDataIt->state.initialize(*(*robotIt)); + robotDataIt->statePrev.initialize(*(*robotIt)); + robotDataIt->successiveSolveFailed = 0U; + } + + // Initialize the ode solver + auto robotOde = [this](double t, + const std::vector & q, + const std::vector & v, + std::vector & a) -> void + { + this->computeRobotsDynamics(t, q, v, a); + }; + std::vector robots; + robots.reserve(nRobots); + std::transform(robots_.begin(), + robots_.end(), + std::back_inserter(robots), + [](const auto & robot) { return robot.get(); }); + if (engineOptions_->stepper.odeSolver == "runge_kutta_dopri5") + { + stepper_ = std::unique_ptr(new RungeKuttaDOPRIStepper( + robotOde, robots, engineOptions_->stepper.tolAbs, engineOptions_->stepper.tolRel)); + } + else if (engineOptions_->stepper.odeSolver == "runge_kutta_4") + { + stepper_ = std::unique_ptr(new RungeKutta4Stepper(robotOde, robots)); + } + else if (engineOptions_->stepper.odeSolver == "euler_explicit") + { + stepper_ = + std::unique_ptr(new EulerExplicitStepper(robotOde, robots)); + } + + // Initialize the stepper state + const double t = 0.0; + stepperState_.reset(SIMULATION_MIN_TIMESTEP, qSplit, vSplit, aSplit); + + // Initialize previous joints forces and accelerations + contactForcesPrev_.clear(); + contactForcesPrev_.reserve(nRobots); + fPrev_.clear(); + fPrev_.reserve(nRobots); + aPrev_.clear(); + aPrev_.reserve(nRobots); + for (const auto & robot : robots_) + { + contactForcesPrev_.push_back(robot->contactForces_); + fPrev_.push_back(robot->pinocchioData_.f); + aPrev_.push_back(robot->pinocchioData_.a); + } + energy_.resize(nRobots, 0.0); + + // Synchronize the individual robot states with the global stepper state + syncRobotsStateWithStepper(); + + // Update the frame indices associated with the coupling forces + for (auto & force : couplingForces_) + { + force.frameIndex1 = + getFrameIndex(robots_[force.robotIndex1]->pinocchioModel_, force.frameName1); + force.frameIndex2 = + getFrameIndex(robots_[force.robotIndex2]->pinocchioModel_, force.frameName2); + } + + robotIt = robots_.begin(); + robotDataIt = robotDataVec_.begin(); + for (; robotIt != robots_.end(); ++robotIt, ++robotDataIt) + { + // Update the frame indices associated with the impulse forces and force profiles + for (auto & force : robotDataIt->profileForces) + { + force.frameIndex = getFrameIndex((*robotIt)->pinocchioModel_, force.frameName); + } + for (auto & force : robotDataIt->impulseForces) + { + force.frameIndex = getFrameIndex((*robotIt)->pinocchioModel_, force.frameName); + } + + // Initialize the impulse force breakpoint point iterator + robotDataIt->impulseForceBreakpointNextIt = + robotDataIt->impulseForceBreakpoints.begin(); + + // Reset the active set of impulse forces + std::fill(robotDataIt->isImpulseForceActiveVec.begin(), + robotDataIt->isImpulseForceActiveVec.end(), + false); + + // Activate every force impulse starting at t=0 + auto isImpulseForceActiveIt = robotDataIt->isImpulseForceActiveVec.begin(); + auto impulseForceIt = robotDataIt->impulseForces.begin(); + for (; impulseForceIt != robotDataIt->impulseForces.end(); + ++isImpulseForceActiveIt, ++impulseForceIt) + { + if (impulseForceIt->t < STEPPER_MIN_TIMESTEP) + { + *isImpulseForceActiveIt = true; + } + } + + // Compute the forward kinematics for each robot + const Eigen::VectorXd & q = robotDataIt->state.q; + const Eigen::VectorXd & v = robotDataIt->state.v; + const Eigen::VectorXd & a = robotDataIt->state.a; + computeForwardKinematics(*robotIt, q, v, a); + + /* Backup constraint register for fast lookup. + Internal constraints cannot be added/removed at this point. */ + robotDataIt->constraints = (*robotIt)->getConstraints(); + + // Initialize contacts forces in local frame + const std::vector & contactFrameIndices = + (*robotIt)->getContactFrameIndices(); + robotDataIt->contactFrameForces = + ForceVector(contactFrameIndices.size(), pinocchio::Force::Zero()); + const std::vector> & collisionPairIndices = + (*robotIt)->getCollisionPairIndices(); + robotDataIt->collisionBodiesForces.clear(); + robotDataIt->collisionBodiesForces.reserve(collisionPairIndices.size()); + for (const auto & bodyCollisionPairIndices : collisionPairIndices) + { + robotDataIt->collisionBodiesForces.emplace_back(bodyCollisionPairIndices.size(), + pinocchio::Force::Zero()); + } + + /* Initialize some addition buffers used by impulse contact solver. + It must be initialized to zero because 'getJointJacobian' will only update non-zero + coefficients for efficiency. */ + robotDataIt->jointJacobians.resize((*robotIt)->pinocchioModel_.njoints, + Matrix6Xd::Zero(6, (*robotIt)->pinocchioModel_.nv)); + + // Reset the constraints + (*robotIt)->resetConstraints(q, v); + + /* Set Baumgarte stabilization natural frequency for contact constraints + Enable all contact constraints by default, it will be disable automatically if not + in contact. It is useful to start in post-hysteresis state to avoid discontinuities + at init. */ + robotDataIt->constraints.foreach( + [&contactModel = contactModel_, + &enablePositionLimit = (*robotIt)->modelOptions_->joints.enablePositionLimit, + &freq = engineOptions_->contacts.stabilizationFreq]( + const std::shared_ptr & constraint, + ConstraintNodeType node) + { + // Set baumgarte freq for all contact constraints + if (node != ConstraintNodeType::USER) + { + constraint->setBaumgarteFreq(freq); // It cannot fail + } + + // Enable constraints by default + if (contactModel == ContactModelType::CONSTRAINT) + { + switch (node) + { + case ConstraintNodeType::BOUNDS_JOINTS: + if (!enablePositionLimit) + { + return; + } + { + auto & jointConstraint = + static_cast(*constraint.get()); + jointConstraint.setRotationDir(false); + } + [[fallthrough]]; + case ConstraintNodeType::CONTACT_FRAMES: + case ConstraintNodeType::COLLISION_BODIES: + constraint->enable(); + break; + case ConstraintNodeType::USER: + default: + break; + } + } + }); + + if (contactModel_ == ContactModelType::SPRING_DAMPER) + { + // Make sure that the contact forces are bounded for spring-damper model. + // TODO: Rather use something like `10 * m * g` instead of a fix threshold. + double forceMax = 0.0; + for (std::size_t i = 0; i < contactFrameIndices.size(); ++i) + { + auto & constraint = robotDataIt->constraints.contactFrames[i].second; + pinocchio::Force & fextLocal = robotDataIt->contactFrameForces[i]; + computeContactDynamicsAtFrame( + *robotIt, contactFrameIndices[i], constraint, fextLocal); + forceMax = std::max(forceMax, fextLocal.linear().norm()); + } + + for (std::size_t i = 0; i < collisionPairIndices.size(); ++i) + { + for (std::size_t j = 0; j < collisionPairIndices[i].size(); ++j) + { + const pinocchio::PairIndex & collisionPairIndex = + collisionPairIndices[i][j]; + auto & constraint = robotDataIt->constraints.collisionBodies[i][j].second; + pinocchio::Force & fextLocal = robotDataIt->collisionBodiesForces[i][j]; + computeContactDynamicsAtBody( + *robotIt, collisionPairIndex, constraint, fextLocal); + forceMax = std::max(forceMax, fextLocal.linear().norm()); + } + } + + if (forceMax > 1e5) + { + THROW_ERROR( + std::invalid_argument, + "The initial force exceeds 1e5 for at least one contact point, which is " + "forbidden for the sake of numerical stability. Please update the initial " + "state."); + } + } + } + + // Lock the robots. At this point, it is no longer possible to change them anymore. + robotIt = robots_.begin(); + robotDataIt = robotDataVec_.begin(); + for (; robotIt != robots_.end(); ++robotIt, ++robotDataIt) + { + robotDataIt->robotLock = (*robotIt)->getLock(); + } + + // Instantiate the desired LCP solver + robotIt = robots_.begin(); + robotDataIt = robotDataVec_.begin(); + for (; robotIt != robots_.end(); ++robotIt, ++robotDataIt) + { + const std::string & constraintSolverType = engineOptions_->constraints.solver; + switch (CONSTRAINT_SOLVERS_MAP.at(constraintSolverType)) + { + case ConstraintSolverType::PGS: + robotDataIt->constraintSolver = + std::make_unique(&((*robotIt)->pinocchioModel_), + &((*robotIt)->pinocchioData_), + &robotDataIt->constraints, + engineOptions_->contacts.friction, + engineOptions_->contacts.torsion, + engineOptions_->stepper.tolAbs, + engineOptions_->stepper.tolRel, + PGS_MAX_ITERATIONS); + break; + case ConstraintSolverType::UNSUPPORTED: + default: + break; + } + } + + /* Compute the efforts, internal and external forces applied on every robots excluding + user-specified internal dynamics if any. */ + computeAllTerms(t, qSplit, vSplit); + + // Backup all external forces and internal efforts excluding constraint forces + vector_aligned_t fextNoConst; + std::vector uInternalConst; + fextNoConst.reserve(nRobots); + uInternalConst.reserve(nRobots); + for (const auto & robotData : robotDataVec_) + { + fextNoConst.push_back(robotData.state.fExternal); + uInternalConst.push_back(robotData.state.uInternal); + } + + /* Solve algebraic coupling between accelerations, sensors and controllers, by + iterating several times until it (hopefully) converges. */ + bool isFirstIter = true; + for (uint32_t i = 0; i < INIT_ITERATIONS; ++i) + { + robotIt = robots_.begin(); + robotDataIt = robotDataVec_.begin(); + auto fextNoConstIt = fextNoConst.begin(); + auto uInternalConstIt = uInternalConst.begin(); + for (; robotIt != robots_.end(); + ++robotIt, ++robotDataIt, ++fextNoConstIt, ++uInternalConstIt) + { + // Get some robot state proxies + const Eigen::VectorXd & q = robotDataIt->state.q; + const Eigen::VectorXd & v = robotDataIt->state.v; + Eigen::VectorXd & a = robotDataIt->state.a; + Eigen::VectorXd & u = robotDataIt->state.u; + Eigen::VectorXd & command = robotDataIt->state.command; + Eigen::VectorXd & uMotor = robotDataIt->state.uMotor; + Eigen::VectorXd & uInternal = robotDataIt->state.uInternal; + Eigen::VectorXd & uCustom = robotDataIt->state.uCustom; + ForceVector & fext = robotDataIt->state.fExternal; + + // Reset the external forces and internal efforts + fext = *fextNoConstIt; + uInternal = *uInternalConstIt; + + // Compute dynamics + a = computeAcceleration( + *robotIt, *robotDataIt, q, v, u, fext, !isFirstIter, isFirstIter); + + // Make sure there is no nan at this point + if ((a.array() != a.array()).any()) + { + THROW_ERROR(std::runtime_error, + "Impossible to compute the acceleration. Probably a " + "subtree has zero inertia along an articulated axis."); + } + + // Compute all external terms including joints accelerations and forces + computeAllExtraTerms(robots_, robotDataVec_, fPrev_); + + // Compute the sensor data with the updated effort and acceleration + (*robotIt)->computeSensorMeasurements(t, q, v, a, uMotor, fext); + + // Compute the actual motor effort + computeCommand(*robotIt, t, q, v, command); + + // Compute the actual motor effort + (*robotIt)->computeMotorEfforts(t, q, v, a, command); + uMotor = (*robotIt)->getMotorEfforts(); + + // Compute the internal dynamics + uCustom.setZero(); + (*robotIt)->getController()->internalDynamics(t, q, v, uCustom); + + // Compute the total effort vector + u = uInternal + uCustom; + for (const auto & motor : (*robotIt)->getMotors()) + { + const std::size_t motorIndex = motor->getIndex(); + const Eigen::Index motorVelocityIndex = motor->getJointVelocityIndex(); + u[motorVelocityIndex] += uMotor[motorIndex]; + } + } + isFirstIter = false; + } + + // Update sensor data one last time to take into account the actual motor effort + robotIt = robots_.begin(); + robotDataIt = robotDataVec_.begin(); + for (; robotIt != robots_.end(); ++robotIt, ++robotDataIt) + { + const Eigen::VectorXd & q = robotDataIt->state.q; + const Eigen::VectorXd & v = robotDataIt->state.v; + const Eigen::VectorXd & a = robotDataIt->state.a; + const Eigen::VectorXd & uMotor = robotDataIt->state.uMotor; + const ForceVector & fext = robotDataIt->state.fExternal; + (*robotIt)->computeSensorMeasurements(t, q, v, a, uMotor, fext); + } + + // Backend the updated joint accelerations and forces + syncAllAccelerationsAndForces(robots_, contactForcesPrev_, fPrev_, aPrev_); + + // Synchronize the global stepper state with the individual robot states + syncStepperStateWithRobots(); + + // Initialize the last robot states + for (auto & robotData : robotDataVec_) + { + robotData.statePrev = robotData.state; + } + + // Lock the telemetry. At this point it is not possible to register new variables. + configureTelemetry(); + + // Log robots data + for (const auto & robot : robots_) + { + // Backup URDF file + const std::string telemetryUrdfFile = + addCircumfix("urdf_file", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER); + const std::string & urdfFileString = robot->getUrdfAsString(); + telemetrySender_->registerConstant(telemetryUrdfFile, urdfFileString); + + // Backup 'has_freeflyer' option + const std::string telemetrHasFreeflyer = + addCircumfix("has_freeflyer", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER); + telemetrySender_->registerConstant(telemetrHasFreeflyer, + toString(robot->getHasFreeflyer())); + + // Backup mesh package lookup directories + const std::string telemetryMeshPackageDirs = addCircumfix( + "mesh_package_dirs", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER); + std::string meshPackageDirsString; + std::stringstream meshPackageDirsStream; + const std::vector & meshPackageDirs = robot->getMeshPackageDirs(); + copy(meshPackageDirs.begin(), + meshPackageDirs.end(), + std::ostream_iterator(meshPackageDirsStream, ";")); + if (meshPackageDirsStream.peek() != + decltype(meshPackageDirsStream)::traits_type::eof()) + { + meshPackageDirsString = meshPackageDirsStream.str(); + meshPackageDirsString.pop_back(); + } + telemetrySender_->registerConstant(telemetryMeshPackageDirs, meshPackageDirsString); + + // Backup the true and theoretical Pinocchio::Model + std::string key = addCircumfix( + "pinocchio_model", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER); + std::string value = saveToBinary(robot->pinocchioModel_); + telemetrySender_->registerConstant(key, value); + + /* Backup the Pinocchio GeometryModel for collisions and visuals. + It may fail because of missing serialization methods for convex, or because it + cannot fit into memory (return code). Persistent mode is automatically enforced + if no URDF is associated with the robot.*/ + if (engineOptions_->telemetry.isPersistent || urdfFileString.empty()) + { + try + { + key = addCircumfix( + "collision_model", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER); + value = saveToBinary(robot->collisionModel_); + telemetrySender_->registerConstant(key, value); + + key = addCircumfix( + "visual_model", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER); + value = saveToBinary(robot->visualModel_); + telemetrySender_->registerConstant(key, value); + } + catch (const std::exception & e) + { + std::string msg{"Failed to log the collision and/or visual model."}; + if (urdfFileString.empty()) + { + msg += " It will be impossible to replay log files because no URDF " + "file is available as fallback."; + } + msg += "\nRaised from exception: "; + PRINT_WARNING(msg, e.what()); + } + } + } + + // Log all options + GenericConfig allOptions; + for (const auto & robot : robots_) + { + const std::string telemetryRobotOptions = + addCircumfix("robot", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER); + allOptions[telemetryRobotOptions] = robot->getOptions(); + } + allOptions["engine"] = engineOptionsGeneric_; + Json::Value allOptionsJson = convertToJson(allOptions); + Json::StreamWriterBuilder jsonWriter; + jsonWriter["indentation"] = ""; + const std::string allOptionsString = Json::writeString(jsonWriter, allOptionsJson); + telemetrySender_->registerConstant("options", allOptionsString); + + // Write the header: this locks the registration of new variables + telemetryRecorder_->initialize(telemetryData_.get(), getTelemetryTimeUnit()); + + // At this point, consider that the simulation is running + isSimulationRunning_ = true; + } + + std::tuple, + std::map, + std::optional>> + sanitizeInitialData(const std::shared_ptr & robot, + bool isStateTheoretical, + const Eigen::VectorXd & qInit, + const Eigen::VectorXd & vInit, + const std::optional & aInit) + { + // Extract robot name for convenience + const std::string & robotName = robot->getName(); + + // Process initial configuration + std::map qInitMap; + if (isStateTheoretical && robot->modelOptions_->dynamics.enableFlexibleModel) + { + Eigen::VectorXd q0; + robot->getFlexiblePositionFromRigid(qInit, q0); + qInitMap.emplace(robotName, std::move(q0)); + } + else + { + qInitMap.emplace(robotName, qInit); + } + + // Process initial velocity + std::map vInitMap; + if (isStateTheoretical && robot->modelOptions_->dynamics.enableFlexibleModel) + { + Eigen::VectorXd v0; + robot->getFlexibleVelocityFromRigid(vInit, v0); + vInitMap.emplace(robotName, std::move(v0)); + } + else + { + vInitMap.emplace(robotName, vInit); + } + + // Process initial acceleration + std::optional> aInitMap; + if (aInit) + { + aInitMap.emplace(); + if (isStateTheoretical && robot->modelOptions_->dynamics.enableFlexibleModel) + { + Eigen::VectorXd a0; + robot->getFlexibleVelocityFromRigid(*aInit, a0); + aInitMap->emplace(robotName, std::move(a0)); + } + else + { + aInitMap->emplace(robotName, aInit.value()); + } + } + + return {qInitMap, vInitMap, aInitMap}; + } + + void Engine::start(const Eigen::VectorXd & qInit, + const Eigen::VectorXd & vInit, + const std::optional & aInit, + bool isStateTheoretical) + { + // Make sure that a single robot has been added to the engine + if (robots_.size() != 1) + { + THROW_ERROR(bad_control_flow, + "Multi-robot simulation requires specifying the initial state of each " + "robot individually."); + } + + // Pre-process initial state + auto [qInitMap, vInitMap, aInitMap] = + sanitizeInitialData(robots_[0], isStateTheoretical, qInit, vInit, aInit); + + // Start simulation + start(qInitMap, vInitMap, aInitMap); + } + + void Engine::simulate(double tEnd, + const std::map & qInit, + const std::map & vInit, + const std::optional> & aInit, + const AbortSimulationFunction & callback) + { + // Make sure that no simulation is already running + if (robots_.empty()) + { + THROW_ERROR(bad_control_flow, + "No robot to simulate. Please add one before starting simulation."); + } + + // Make sure that the user-specified simulation duration is long enough + if (tEnd < 5e-3) + { + THROW_ERROR(std::invalid_argument, "Simulation duration cannot be shorter than 5ms."); + } + + // Reset the engine and all the robots + reset(true, false); + + // Start the simulation + start(qInit, vInit, aInit); + + // Now that telemetry has been initialized, check simulation duration + if (tEnd > telemetryRecorder_->getLogDurationMax()) + { + THROW_ERROR(std::runtime_error, + "Time overflow: with the current precision the maximum value that can be " + "logged is ", + telemetryRecorder_->getLogDurationMax(), + "s. Decrease logger precision to simulate for longer than that."); + } + + // Integration loop based on boost::numeric::odeint::detail::integrate_times + while (true) + { + // Stop the simulation if the end time has been reached + if (tEnd - stepperState_.t < SIMULATION_MIN_TIMESTEP) + { + if (engineOptions_->stepper.verbose) + { + std::cout << "Simulation done: desired final time reached." << std::endl; + } + break; + } + + // Stop the simulation if callback returns false + if (!callback()) + { + if (engineOptions_->stepper.verbose) + { + std::cout << "Simulation done: callback returned false." << std::endl; + } + break; + } + + // Stop the simulation if the max number of integration steps is reached + if (0U < engineOptions_->stepper.iterMax && + engineOptions_->stepper.iterMax <= stepperState_.iter) + { + if (engineOptions_->stepper.verbose) + { + std::cout << "Simulation done: maximum number of integration steps exceeded." + << std::endl; + } + break; + } + + // Perform a single integration step up to `tEnd`, stopping at `stepperUpdatePeriod_` + double stepSize; + if (std::isfinite(stepperUpdatePeriod_)) + { + stepSize = std::min(stepperUpdatePeriod_, tEnd - stepperState_.t); + } + else + { + stepSize = std::min(engineOptions_->stepper.dtMax, tEnd - stepperState_.t); + } + step(stepSize); // Automatic dt adjustment + } + + // Stop the simulation. The lock on the telemetry and the robots are released. + stop(); + } + + void Engine::simulate(double tEnd, + const Eigen::VectorXd & qInit, + const Eigen::VectorXd & vInit, + const std::optional & aInit, + bool isStateTheoretical, + const AbortSimulationFunction & callback) + { + // Make sure that a single robot has been added to the engine + if (robots_.size() != 1) + { + THROW_ERROR(bad_control_flow, + "Multi-robot simulation requires specifying the initial state of each " + "robot individually."); + } + + // Pre-process initial state + auto [qInitMap, vInitMap, aInitMap] = + sanitizeInitialData(robots_[0], isStateTheoretical, qInit, vInit, aInit); + + // Run simulation + simulate(tEnd, qInitMap, vInitMap, aInitMap, callback); + } + + void Engine::step(double stepSize) + { + // Check if the simulation has started + if (!isSimulationRunning_) + { + THROW_ERROR(bad_control_flow, + "No simulation running. Please start one before using step method."); + } + + // Clear log data buffer + logData_.reset(); + + // Check if there is something wrong with the integration + auto qIt = stepperState_.qSplit.begin(); + auto vIt = stepperState_.vSplit.begin(); + auto aIt = stepperState_.aSplit.begin(); + for (; qIt != stepperState_.qSplit.end(); ++qIt, ++vIt, ++aIt) + { + if (qIt->hasNaN() || vIt->hasNaN() || aIt->hasNaN()) + { + THROW_ERROR(std::runtime_error, + "Low-level ode solver failed. Consider increasing stepper accuracy."); + } + } + + // Check if the desired step size is suitable + if (stepSize > EPS && stepSize < SIMULATION_MIN_TIMESTEP) + { + THROW_ERROR(std::invalid_argument, "Step size out of bounds."); + } + + /* Set end time: The default step size is equal to the controller update period if + discrete-time, otherwise it uses the sensor update period if discrete-time, otherwise + it uses the user-defined parameter dtMax. */ + if (stepSize < EPS) + { + const double controllerUpdatePeriod = engineOptions_->stepper.controllerUpdatePeriod; + if (controllerUpdatePeriod > EPS) + { + stepSize = controllerUpdatePeriod; + } + else + { + const double sensorsUpdatePeriod = engineOptions_->stepper.sensorsUpdatePeriod; + if (sensorsUpdatePeriod > EPS) + { + stepSize = sensorsUpdatePeriod; + } + else + { + stepSize = engineOptions_->stepper.dtMax; + } + } + } + + /* Check that end time is not too large for the current logging precision, otherwise abort + integration. */ + if (stepperState_.t + stepSize > telemetryRecorder_->getLogDurationMax()) + { + THROW_ERROR(std::runtime_error, + "Time overflow: with the current precision the maximum value that can be " + "logged is ", + telemetryRecorder_->getLogDurationMax(), + "s. Decrease logger precision to simulate for longer than that."); + } + + /* Avoid compounding of error using Kahan algorithm. It consists in keeping track of the + cumulative rounding error to add it back to the sum when it gets larger than the + numerical precision, thus avoiding it to grows unbounded. */ + const double stepSizeCorrected = stepSize - stepperState_.tError; + const double tEnd = stepperState_.t + stepSizeCorrected; + stepperState_.tError = (tEnd - stepperState_.t) - stepSizeCorrected; + + // Get references to some internal stepper buffers + double & t = stepperState_.t; + double & dt = stepperState_.dt; + double & dtLargest = stepperState_.dtLargest; + std::vector & qSplit = stepperState_.qSplit; + std::vector & vSplit = stepperState_.vSplit; + std::vector & aSplit = stepperState_.aSplit; + + // Monitor iteration failure + uint32_t successiveIterFailed = 0; + std::vector successiveSolveFailedAll(robots_.size(), 0U); + bool isNan = false; + + /* Flag monitoring if the current time step depends of a breakpoint or the integration + tolerance. It will be used by the restoration mechanism, if dt gets very small to reach + a breakpoint, in order to avoid having to perform several steps to stabilize again the + estimation of the optimal time step. */ + bool isBreakpointReached = false; + + /* Flag monitoring if the dynamics has changed because of impulse forces or the command + (only in the case of discrete control). + + `tryStep(rhs, x, dxdt, t, dt)` method of error controlled boost steppers leverage the + FSAL (first same as last) principle. It is implemented by considering at the value of + (x, dxdt) in argument have been initialized by the user with the robot dynamics at + current time t. Thus, if the robot dynamics is discontinuous, one has to manually + integrate up to t-, then update dxdt to take into the acceleration at t+. + + Note that ONLY the acceleration part of dxdt must be updated since the velocity is not + supposed to have changed. On top of that, tPrev is invalid at this point because it has + been updated just after the last successful step. + + TODO: Maybe dt should be reschedule because the dynamics has changed and thereby the + previously estimated dt is very meaningful anymore. */ + bool hasDynamicsChanged = false; + + // Start the timer used for timeout handling + timer_.tic(); + + // Perform the integration. Do not simulate extremely small time steps. + while (tEnd - t >= STEPPER_MIN_TIMESTEP) + { + // Initialize next breakpoint time to the one recommended by the stepper + double tNext = t; + + // Update the active set and get the next breakpoint of impulse forces + double tImpulseForceNext = INF; + for (auto & robotData : robotDataVec_) + { + /* Update the active set: activate an impulse force as soon as the current time + gets close enough of the application time, and deactivate it once the following + the same reasoning. + + Note that breakpoints at the start/end of every impulse forces are already + enforced, so that the forces cannot get activated/deactivate too late. */ + auto isImpulseForceActiveIt = robotData.isImpulseForceActiveVec.begin(); + auto impulseForceIt = robotData.impulseForces.begin(); + for (; impulseForceIt != robotData.impulseForces.end(); + ++isImpulseForceActiveIt, ++impulseForceIt) + { + double tImpulseForce = impulseForceIt->t; + double dtImpulseForce = impulseForceIt->dt; + + if (t > tImpulseForce - STEPPER_MIN_TIMESTEP) + { + *isImpulseForceActiveIt = true; + hasDynamicsChanged = true; + } + if (t >= tImpulseForce + dtImpulseForce - STEPPER_MIN_TIMESTEP) + { + *isImpulseForceActiveIt = false; + hasDynamicsChanged = true; + } + } + + // Update the breakpoint time iterator if necessary + auto & tBreakpointNextIt = robotData.impulseForceBreakpointNextIt; + if (tBreakpointNextIt != robotData.impulseForceBreakpoints.end()) + { + if (t >= *tBreakpointNextIt - STEPPER_MIN_TIMESTEP) + { + // The current breakpoint is behind in time. Switching to the next one. + ++tBreakpointNextIt; + } + } + + // Get the next breakpoint time if any + if (tBreakpointNextIt != robotData.impulseForceBreakpoints.end()) + { + tImpulseForceNext = std::min(tImpulseForceNext, *tBreakpointNextIt); + } + } + + // Update the external force profiles if necessary (only for finite update frequency) + if (std::isfinite(stepperUpdatePeriod_)) + { + auto robotIt = robots_.begin(); + auto robotDataIt = robotDataVec_.begin(); + for (; robotIt != robots_.end(); ++robotIt, ++robotDataIt) + { + for (auto & profileForce : robotDataIt->profileForces) + { + if (profileForce.updatePeriod > EPS) + { + double forceUpdatePeriod = profileForce.updatePeriod; + double dtNextForceUpdatePeriod = + forceUpdatePeriod - std::fmod(t, forceUpdatePeriod); + if (dtNextForceUpdatePeriod < SIMULATION_MIN_TIMESTEP || + forceUpdatePeriod - dtNextForceUpdatePeriod < STEPPER_MIN_TIMESTEP) + { + const Eigen::VectorXd & q = robotDataIt->state.q; + const Eigen::VectorXd & v = robotDataIt->state.v; + profileForce.force = profileForce.func(t, q, v); + hasDynamicsChanged = true; + } + } + } + } + } + + // Update the controller command if necessary (only for finite update frequency) + if (std::isfinite(stepperUpdatePeriod_) && + engineOptions_->stepper.controllerUpdatePeriod > EPS) + { + double controllerUpdatePeriod = engineOptions_->stepper.controllerUpdatePeriod; + double dtNextControllerUpdatePeriod = + controllerUpdatePeriod - std::fmod(t, controllerUpdatePeriod); + if (dtNextControllerUpdatePeriod < SIMULATION_MIN_TIMESTEP || + controllerUpdatePeriod - dtNextControllerUpdatePeriod < STEPPER_MIN_TIMESTEP) + { + auto robotIt = robots_.begin(); + auto robotDataIt = robotDataVec_.begin(); + for (; robotIt != robots_.end(); ++robotIt, ++robotDataIt) + { + const Eigen::VectorXd & q = robotDataIt->state.q; + const Eigen::VectorXd & v = robotDataIt->state.v; + Eigen::VectorXd & command = robotDataIt->state.command; + computeCommand(*robotIt, t, q, v, command); + } + hasDynamicsChanged = true; + } + } + + /* Update telemetry if necessary. It monitors the current iteration number, the current + time, and the robots state, command, and sensors data. + + Note that the acceleration is discontinuous. In particular, it would have different + values of the same timestep if the command has been updated. There is no way to log + both the acceleration at the end of the previous step (t-) and at the beginning of + the next one (t+). Logging the previous acceleration is more natural since it + preserves the consistency between sensors data and robot state. */ + if (!std::isfinite(stepperUpdatePeriod_) || + !engineOptions_->stepper.logInternalStepperSteps) + { + bool mustUpdateTelemetry = !std::isfinite(stepperUpdatePeriod_); + if (!mustUpdateTelemetry) + { + double dtNextStepperUpdatePeriod = + stepperUpdatePeriod_ - std::fmod(t, stepperUpdatePeriod_); + mustUpdateTelemetry = + (dtNextStepperUpdatePeriod < SIMULATION_MIN_TIMESTEP || + stepperUpdatePeriod_ - dtNextStepperUpdatePeriod < STEPPER_MIN_TIMESTEP); + } + if (mustUpdateTelemetry) + { + updateTelemetry(); + } + } + + // Fix the FSAL issue if the dynamics has changed + if (!std::isfinite(stepperUpdatePeriod_) && hasDynamicsChanged) + { + computeRobotsDynamics(t, qSplit, vSplit, aSplit, true); + syncAllAccelerationsAndForces(robots_, contactForcesPrev_, fPrev_, aPrev_); + syncRobotsStateWithStepper(true); + hasDynamicsChanged = false; + } + + if (std::isfinite(stepperUpdatePeriod_)) + { + /* Get the time of the next breakpoint for the ODE solver: a breakpoint occurs if: + - tEnd has been reached + - an external impulse force must be activated/deactivated + - the sensor measurements or the controller command must be updated. */ + double dtNextGlobal; // dt to apply for the next stepper step + const double dtNextUpdatePeriod = + stepperUpdatePeriod_ - std::fmod(t, stepperUpdatePeriod_); + if (dtNextUpdatePeriod < SIMULATION_MIN_TIMESTEP) + { + /* Step to reach next sensors/controller update is too short: skip one + controller update and jump to the next one. + + Note that in this case, the sensors have already been updated in advance + during the previous loop. */ + dtNextGlobal = + std::min(dtNextUpdatePeriod + stepperUpdatePeriod_, tImpulseForceNext - t); + } + else + { + dtNextGlobal = std::min(dtNextUpdatePeriod, tImpulseForceNext - t); + } + + /* Check if the next dt to about equal to the time difference between the current + time (it can only be smaller) and enforce next dt to exactly match this value in + such a case. */ + if (tEnd - t - STEPPER_MIN_TIMESTEP < dtNextGlobal) + { + dtNextGlobal = tEnd - t; + } + + // Update next dt + tNext += dtNextGlobal; + + // Compute the next step using adaptive step method + while (tNext - t > STEPPER_MIN_TIMESTEP) + { + // Log every stepper state only if the user asked for + if (successiveIterFailed == 0 && + engineOptions_->stepper.logInternalStepperSteps) + { + updateTelemetry(); + } + + // Fix the FSAL issue if the dynamics has changed + if (hasDynamicsChanged) + { + computeRobotsDynamics(t, qSplit, vSplit, aSplit, true); + syncAllAccelerationsAndForces(robots_, contactForcesPrev_, fPrev_, aPrev_); + syncRobotsStateWithStepper(true); + hasDynamicsChanged = false; + } + + /* Adjust stepsize to end up exactly at the next breakpoint if it is reasonable + to expect that integration will be successful, namely: + - If the next breakpoint is closer than the estimated maximum step size + OR + - If the next breakpoint is farther but not so far away compared to the + estimated maximum step size, AND the previous integration trial was + successful. This last condition is essential to prevent infinite loop of + slightly increasing the step size, failing to integrate, then try again + and again until triggering maximum successive iteration failure exception. + The current implementation is conservative and does not check that the + previous failure was due to this stepsize adjustment procedure, but it is + just a performance optimization trick, so it should not be a big deal. */ + const double dtResidualThr = + std::min(SIMULATION_MIN_TIMESTEP, 0.1 * dtLargest); + if (tNext - t < dt || + (successiveIterFailed == 0 && tNext - t < dt + dtResidualThr)) + { + dt = tNext - t; + } + + /* Trying to reach multiples of STEPPER_MIN_TIMESTEP whenever possible. The + idea here is to reach only multiples of 1us, making logging easier, given + that, 1us can be consider an 'infinitesimal' time in robotics. This + arbitrary threshold many not be suited for simulating different, faster + dynamics, that require sub-microsecond precision. */ + if (dt > SIMULATION_MIN_TIMESTEP) + { + const double dtResidual = std::fmod(dt, SIMULATION_MIN_TIMESTEP); + if (dtResidual > STEPPER_MIN_TIMESTEP && + dtResidual < SIMULATION_MIN_TIMESTEP - STEPPER_MIN_TIMESTEP && + dt - dtResidual > STEPPER_MIN_TIMESTEP) + { + dt -= dtResidual; + } + } + + // Break the loop if dt is getting too small. The error code will be set later. + if (dt < STEPPER_MIN_TIMESTEP) + { + break; + } + + // Break the loop in case of timeout. The error code will be set later. + if (EPS < engineOptions_->stepper.timeout && + engineOptions_->stepper.timeout < timer_.toc()) + { + break; + } + + // Break the loop in case of too many successive failed inner iteration + if (successiveIterFailed > engineOptions_->stepper.successiveIterFailedMax) + { + break; + } + + /* Backup current number of successive constraint solving failure. + It will be restored in case of integration failure. */ + auto robotDataIt = robotDataVec_.begin(); + auto successiveSolveFailedIt = successiveSolveFailedAll.begin(); + for (; robotDataIt != robotDataVec_.end(); + ++robotDataIt, ++successiveSolveFailedIt) + { + *successiveSolveFailedIt = robotDataIt->successiveSolveFailed; + } + + // Break the loop in case of too many successive constraint solving failures + for (uint32_t successiveSolveFailed : successiveSolveFailedAll) + { + if (successiveSolveFailed > + engineOptions_->stepper.successiveIterFailedMax) + { + break; + } + } + + /* A breakpoint has been reached, causing dt to be smaller that necessary for + prescribed integration tol. */ + isBreakpointReached = (dtLargest > dt); + + // Set the timestep to be tried by the stepper + dtLargest = dt; + + // Try doing one integration step + bool isStepSuccessful = + stepper_->tryStep(qSplit, vSplit, aSplit, t, dtLargest); + + /* Check if the integrator failed miserably even if successfully. + It would happen the timestep is fixed and too large, causing the integrator + to fail miserably returning nan. */ + isNan = std::isnan(dtLargest); + if (isNan) + { + break; + } + + // Update buffer if really successful + if (isStepSuccessful) + { + // Reset successive iteration failure counter + successiveIterFailed = 0; + + // Synchronize the position, velocity and acceleration of all robots + syncRobotsStateWithStepper(); + + /* Compute all external terms including joints accelerations and forces. + Note that it is possible to call this method because `pinocchio::Data` + is guaranteed to be up-to-date at this point. */ + computeAllExtraTerms(robots_, robotDataVec_, fPrev_); + + // Backend the updated joint accelerations and forces + syncAllAccelerationsAndForces(robots_, contactForcesPrev_, fPrev_, aPrev_); + + // Increment the iteration counter only for successful steps + ++stepperState_.iter; + + /* Restore the step size dt if it has been significantly decreased because + of a breakpoint. It is set equal to the last available largest dt to be + known, namely the second to last successful step. */ + if (isBreakpointReached) + { + /* Restore the step size if and only if: + - the next estimated largest step size is larger than the requested + one for the current (successful) step. + - the next estimated largest step size is significantly smaller than + the estimated largest step size for the previous step. */ + double dtRestoreThresholdAbs = + stepperState_.dtLargestPrev * + engineOptions_->stepper.dtRestoreThresholdRel; + if (dt < dtLargest && dtLargest < dtRestoreThresholdAbs) + { + dtLargest = stepperState_.dtLargestPrev; + } + } + + /* Backup the stepper and robots' state on success only: + - t at last successful iteration is used to compute dt, which is project + the acceleration in the state space instead of SO3^2. + - dtLargestPrev is used to restore the largest step size in case of a + breakpoint requiring lowering it. + - the acceleration and effort at the last successful iteration is used + to update the sensors' data in case of continuous sensing. */ + stepperState_.tPrev = t; + stepperState_.dtLargestPrev = dtLargest; + for (auto & robotData : robotDataVec_) + { + robotData.statePrev = robotData.state; + } + } + else + { + // Increment the failed iteration counters + ++successiveIterFailed; + ++stepperState_.iterFailed; + + // Restore number of successive constraint solving failure + robotDataIt = robotDataVec_.begin(); + successiveSolveFailedIt = successiveSolveFailedAll.begin(); + for (; robotDataIt != robotDataVec_.end(); + ++robotDataIt, ++successiveSolveFailedIt) + { + robotDataIt->successiveSolveFailed = *successiveSolveFailedIt; + } + } + + // Initialize the next dt + dt = std::min(dtLargest, engineOptions_->stepper.dtMax); + } + } + else + { + /* Make sure it ends exactly at the tEnd, never exceeds dtMax, and stop to apply + impulse forces. */ + dt = std::min({dt, tEnd - t, tImpulseForceNext - t}); + + /* A breakpoint has been reached, because dt has been decreased wrt the largest + possible dt within integration tol. */ + isBreakpointReached = (dtLargest > dt); + + // Compute the next step using adaptive step method + bool isStepSuccessful = false; + while (!isStepSuccessful) + { + // Set the timestep to be tried by the stepper + dtLargest = dt; + + // Break the loop in case of too many successive failed inner iteration + if (successiveIterFailed > engineOptions_->stepper.successiveIterFailedMax) + { + break; + } + + /* Backup current number of successive constraint solving failure. + It will be restored in case of integration failure. */ + auto robotDataIt = robotDataVec_.begin(); + auto successiveSolveFailedIt = successiveSolveFailedAll.begin(); + for (; robotDataIt != robotDataVec_.end(); + ++robotDataIt, ++successiveSolveFailedIt) + { + *successiveSolveFailedIt = robotDataIt->successiveSolveFailed; + } + + // Break the loop in case of too many successive constraint solving failures + for (uint32_t successiveSolveFailed : successiveSolveFailedAll) + { + if (successiveSolveFailed > + engineOptions_->stepper.successiveIterFailedMax) + { + break; + } + } + + // Try to do a step + isStepSuccessful = stepper_->tryStep(qSplit, vSplit, aSplit, t, dtLargest); + + // Check if the integrator failed miserably even if successfully + isNan = std::isnan(dtLargest); + if (isNan) + { + break; + } + + if (isStepSuccessful) + { + // Reset successive iteration failure counter + successiveIterFailed = 0; + + // Synchronize the position, velocity and acceleration of all robots + syncRobotsStateWithStepper(); + + // Compute all external terms including joints accelerations and forces + computeAllExtraTerms(robots_, robotDataVec_, fPrev_); + + // Backend the updated joint accelerations and forces + syncAllAccelerationsAndForces(robots_, contactForcesPrev_, fPrev_, aPrev_); + + // Increment the iteration counter + ++stepperState_.iter; + + // Restore the step size if necessary + if (isBreakpointReached) + { + double dtRestoreThresholdAbs = + stepperState_.dtLargestPrev * + engineOptions_->stepper.dtRestoreThresholdRel; + if (dt < dtLargest && dtLargest < dtRestoreThresholdAbs) + { + dtLargest = stepperState_.dtLargestPrev; + } + } + + // Backup the stepper and robots' state + stepperState_.tPrev = t; + stepperState_.dtLargestPrev = dtLargest; + for (auto & robotData : robotDataVec_) + { + robotData.statePrev = robotData.state; + } + } + else + { + // Increment the failed iteration counter + ++successiveIterFailed; + ++stepperState_.iterFailed; + + // Restore number of successive constraint solving failure + robotDataIt = robotDataVec_.begin(); + successiveSolveFailedIt = successiveSolveFailedAll.begin(); + for (; robotDataIt != robotDataVec_.end(); + ++robotDataIt, ++successiveSolveFailedIt) + { + robotDataIt->successiveSolveFailed = *successiveSolveFailedIt; + } + } + + // Initialize the next dt + dt = std::min(dtLargest, engineOptions_->stepper.dtMax); + } + } + + // Exception handling + if (isNan) + { + THROW_ERROR(std::runtime_error, + "Something is wrong with the physics. Aborting integration."); + } + if (successiveIterFailed > engineOptions_->stepper.successiveIterFailedMax) + { + THROW_ERROR(std::runtime_error, + "Too many successive iteration failures. Probably something " + "is going wrong with the physics. Aborting integration."); + } + for (uint32_t successiveSolveFailed : successiveSolveFailedAll) + { + if (successiveSolveFailed > engineOptions_->stepper.successiveIterFailedMax) + { + THROW_ERROR( + std::runtime_error, + "Too many successive constraint solving failures. Try increasing the " + "regularization factor. Aborting integration."); + } + } + if (dt < STEPPER_MIN_TIMESTEP) + { + THROW_ERROR(std::runtime_error, + "The internal time step is getting too small. Impossible to " + "integrate physics further in time. Aborting integration."); + } + if (EPS < engineOptions_->stepper.timeout && + engineOptions_->stepper.timeout < timer_.toc()) + { + THROW_ERROR(std::runtime_error, "Step computation timeout. Aborting integration."); + } + + // Update sensors data if necessary, namely if time-continuous or breakpoint + const double sensorsUpdatePeriod = engineOptions_->stepper.sensorsUpdatePeriod; + const double dtNextSensorsUpdatePeriod = + sensorsUpdatePeriod - std::fmod(t, sensorsUpdatePeriod); + bool mustUpdateSensors = sensorsUpdatePeriod < EPS; + if (!mustUpdateSensors) + { + mustUpdateSensors = dtNextSensorsUpdatePeriod < SIMULATION_MIN_TIMESTEP || + sensorsUpdatePeriod - dtNextSensorsUpdatePeriod < + STEPPER_MIN_TIMESTEP; + } + if (mustUpdateSensors) + { + auto robotIt = robots_.begin(); + auto robotDataIt = robotDataVec_.begin(); + for (; robotIt != robots_.end(); ++robotIt, ++robotDataIt) + { + const Eigen::VectorXd & q = robotDataIt->state.q; + const Eigen::VectorXd & v = robotDataIt->state.v; + const Eigen::VectorXd & a = robotDataIt->state.a; + const Eigen::VectorXd & uMotor = robotDataIt->state.uMotor; + const ForceVector & fext = robotDataIt->state.fExternal; + (*robotIt)->computeSensorMeasurements(t, q, v, a, uMotor, fext); + } + } + } + + /* Update the final time and dt to make sure it corresponds to the desired values and avoid + compounding of error. Anyway the user asked for a step of exactly stepSize, so he is + expecting this value to be reached. */ + t = tEnd; + } + + void Engine::stop() + { + // Release the lock on the robots + for (auto & robotData : robotDataVec_) + { + robotData.robotLock.reset(nullptr); + } + + // Make sure that a simulation running + if (!isSimulationRunning_) + { + return; + } + + // Log current buffer content as final point of the log data + updateTelemetry(); + + // Clear log data buffer one last time, now that the final point has been added + logData_.reset(); + + /* Reset the telemetry. + Note that calling ``stop` or `reset` does NOT clear the internal data buffer of + telemetryRecorder_. Clearing is done at init time, so that it remains accessible until + the next initialization. */ + telemetryRecorder_->reset(); + telemetryData_->reset(); + + // Update some internal flags + isSimulationRunning_ = false; + } + + void Engine::registerImpulseForce(const std::string & robotName, + const std::string & frameName, + double t, + double dt, + const pinocchio::Force & force) + { + if (isSimulationRunning_) + { + THROW_ERROR( + bad_control_flow, + "Simulation already running. Please stop it before registering new forces."); + } + + if (dt < STEPPER_MIN_TIMESTEP) + { + THROW_ERROR(std::invalid_argument, + "Force duration cannot be smaller than ", + STEPPER_MIN_TIMESTEP, + "s."); + } + + if (t < 0.0) + { + THROW_ERROR(std::invalid_argument, "Force application time must be positive."); + } + + if (frameName == "universe") + { + THROW_ERROR(std::invalid_argument, + "Impossible to apply external forces to the universe itself!"); + } + + // TODO: Make sure that the forces do NOT overlap while taking into account dt. + + std::ptrdiff_t robotIndex = getRobotIndex(robotName); + pinocchio::FrameIndex frameIndex = + getFrameIndex(robots_[robotIndex]->pinocchioModel_, frameName); + + RobotData & robotData = robotDataVec_[robotIndex]; + robotData.impulseForces.emplace_back(frameName, frameIndex, t, dt, force); + robotData.impulseForceBreakpoints.emplace(t); + robotData.impulseForceBreakpoints.emplace(t + dt); + robotData.isImpulseForceActiveVec.emplace_back(false); + } + + template + std::tuple + isGcdIncluded(const vector_aligned_t & robotDataVec, const Args &... values) + { + if (robotDataVec.empty()) + { + return isGcdIncluded(values...); + } + + const double * valueMinPtr = &INF; + auto func = [&valueMinPtr, &values...](const RobotData & robotData) + { + auto && [isIncluded, value] = isGcdIncluded( + robotData.profileForces.cbegin(), + robotData.profileForces.cend(), + [](const ProfileForce & force) -> const double & { return force.updatePeriod; }, + values...); + valueMinPtr = &(minClipped(*valueMinPtr, value)); + return isIncluded; + }; + // FIXME: Order of evaluation is not always respected with MSVC. + bool isIncluded = std::all_of(robotDataVec.begin(), robotDataVec.end(), func); + return {isIncluded, *valueMinPtr}; + } + + void Engine::registerProfileForce(const std::string & robotName, + const std::string & frameName, + const ProfileForceFunction & forceFunc, + double updatePeriod) + { + if (isSimulationRunning_) + { + THROW_ERROR( + bad_control_flow, + "Simulation already running. Please stop it before registering new forces."); + } + + if (frameName == "universe") + { + THROW_ERROR(std::invalid_argument, + "Impossible to apply external forces to the universe itself!"); + } + + // Get robot and frame indices + std::ptrdiff_t robotIndex = getRobotIndex(robotName); + pinocchio::FrameIndex frameIndex = + getFrameIndex(robots_[robotIndex]->pinocchioModel_, frameName); + + // Make sure the update period is valid + if (EPS < updatePeriod && updatePeriod < SIMULATION_MIN_TIMESTEP) + { + THROW_ERROR( + std::invalid_argument, + "Cannot register external force profile with update period smaller than ", + SIMULATION_MIN_TIMESTEP, + "s. Adjust period or switch to continuous mode by setting period to zero."); + } + // Make sure the desired update period is a multiple of the stepper period + auto [isIncluded, updatePeriodMin] = + isGcdIncluded(robotDataVec_, stepperUpdatePeriod_, updatePeriod); + if (!isIncluded) + { + THROW_ERROR(std::invalid_argument, + "In discrete mode, the update period of force profiles and the " + "stepper update period (min of controller and sensor update " + "periods) must be multiple of each other."); + } + + // Set breakpoint period during the integration loop + stepperUpdatePeriod_ = updatePeriodMin; + + // Add force profile to register + RobotData & robotData = robotDataVec_[robotIndex]; + robotData.profileForces.emplace_back(frameName, frameIndex, updatePeriod, forceFunc); + } + + void Engine::removeImpulseForces(const std::string & robotName) + { + // Make sure that no simulation is running + if (isSimulationRunning_) + { + THROW_ERROR(bad_control_flow, + "Simulation already running. Stop it before removing coupling forces."); + } + + std::ptrdiff_t robotIndex = getRobotIndex(robotName); + RobotData & robotData = robotDataVec_[robotIndex]; + robotData.impulseForces.clear(); + } + + void Engine::removeImpulseForces() + { + // Make sure that no simulation is running + if (isSimulationRunning_) + { + THROW_ERROR(bad_control_flow, + "simulation already running. Stop it before removing coupling forces."); + } + + for (auto & robotData : robotDataVec_) + { + robotData.impulseForces.clear(); + } + } + + void Engine::removeProfileForces(const std::string & robotName) + { + // Make sure that no simulation is running + if (isSimulationRunning_) + { + THROW_ERROR(bad_control_flow, + "Simulation already running. Stop it before removing coupling forces."); + } + + + // Remove force profile from register + std::ptrdiff_t robotIndex = getRobotIndex(robotName); + RobotData & robotData = robotDataVec_[robotIndex]; + robotData.profileForces.clear(); + + // Set breakpoint period during the integration loop + // FIXME: replaced `std::get` by placeholder `_` when moving to C++26 (P2169R4) + stepperUpdatePeriod_ = + std::get<1>(isGcdIncluded(robotDataVec_, + engineOptions_->stepper.sensorsUpdatePeriod, + engineOptions_->stepper.controllerUpdatePeriod)); + } + + void Engine::removeProfileForces() + { + // Make sure that no simulation is running + if (isSimulationRunning_) + { + THROW_ERROR(bad_control_flow, + "Simulation already running. Stop it before removing coupling forces."); + } + + for (auto & robotData : robotDataVec_) + { + robotData.profileForces.clear(); + } + } + + const ImpulseForceVector & Engine::getImpulseForces(const std::string & robotName) const + { + std::ptrdiff_t robotIndex = getRobotIndex(robotName); + const RobotData & robotData = robotDataVec_[robotIndex]; + return robotData.impulseForces; + } + + const ProfileForceVector & Engine::getProfileForces(const std::string & robotName) const + { + std::ptrdiff_t robotIndex = getRobotIndex(robotName); + const RobotData & robotData = robotDataVec_[robotIndex]; + return robotData.profileForces; + } + + GenericConfig Engine::getOptions() const noexcept + { + return engineOptionsGeneric_; + } + + void Engine::setOptions(const GenericConfig & engineOptions) + { + if (isSimulationRunning_) + { + THROW_ERROR(bad_control_flow, + "Simulation already running. Please stop it before updating the options."); + } + + // Make sure the dtMax is not out of range + GenericConfig stepperOptions = boost::get(engineOptions.at("stepper")); + const double dtMax = boost::get(stepperOptions.at("dtMax")); + if (SIMULATION_MAX_TIMESTEP + EPS < dtMax || dtMax < SIMULATION_MIN_TIMESTEP) + { + THROW_ERROR(std::invalid_argument, + "'dtMax' option must bge in range [", + SIMULATION_MIN_TIMESTEP, + ", ", + SIMULATION_MAX_TIMESTEP, + "]."); + } + + // Make sure successiveIterFailedMax is strictly positive + const uint32_t successiveIterFailedMax = + boost::get(stepperOptions.at("successiveIterFailedMax")); + if (successiveIterFailedMax < 1) + { + THROW_ERROR(std::invalid_argument, + "'successiveIterFailedMax' must be strictly positive."); + } + + // Make sure the selected ode solver is available and instantiate it + const std::string & odeSolver = boost::get(stepperOptions.at("odeSolver")); + if (STEPPERS.find(odeSolver) == STEPPERS.end()) + { + THROW_ERROR( + std::invalid_argument, "Requested ODE solver '", odeSolver, "' not available."); + } + + // Make sure the controller and sensor update periods are valid + const double sensorsUpdatePeriod = + boost::get(stepperOptions.at("sensorsUpdatePeriod")); + const double controllerUpdatePeriod = + boost::get(stepperOptions.at("controllerUpdatePeriod")); + auto [isIncluded, updatePeriodMin] = + isGcdIncluded(robotDataVec_, controllerUpdatePeriod, sensorsUpdatePeriod); + if ((EPS < sensorsUpdatePeriod && sensorsUpdatePeriod < SIMULATION_MIN_TIMESTEP) || + (EPS < controllerUpdatePeriod && controllerUpdatePeriod < SIMULATION_MIN_TIMESTEP)) + { + THROW_ERROR( + std::invalid_argument, + "Cannot simulate a discrete robot with update period smaller than ", + SIMULATION_MIN_TIMESTEP, + "s. Adjust period or switch to continuous mode by setting period to zero."); + } + else if (!isIncluded) + { + THROW_ERROR(std::invalid_argument, + "In discrete mode, the controller and sensor update " + "periods must be multiple of each other."); + } + + // Make sure the contacts options are fine + GenericConfig constraintsOptions = + boost::get(engineOptions.at("constraints")); + const std::string & constraintSolverType = + boost::get(constraintsOptions.at("solver")); + const auto constraintSolverIt = CONSTRAINT_SOLVERS_MAP.find(constraintSolverType); + if (constraintSolverIt == CONSTRAINT_SOLVERS_MAP.end()) + { + THROW_ERROR(std::invalid_argument, + "Requested constraint solver '", + constraintSolverType, + "' not available."); + } + double regularization = boost::get(constraintsOptions.at("regularization")); + if (regularization < 0.0) + { + THROW_ERROR(std::invalid_argument, + "Constraint option 'regularization' must be positive."); + } + + // Make sure the contacts options are fine + GenericConfig contactOptions = boost::get(engineOptions.at("contacts")); + const std::string & contactModel = boost::get(contactOptions.at("model")); + const auto contactModelIt = CONTACT_MODELS_MAP.find(contactModel); + if (contactModelIt == CONTACT_MODELS_MAP.end()) + { + THROW_ERROR(std::invalid_argument, "Requested contact model not available."); + } + double contactsTransitionEps = boost::get(contactOptions.at("transitionEps")); + if (contactsTransitionEps < 0.0) + { + THROW_ERROR(std::invalid_argument, "Contact option 'transitionEps' must be positive."); + } + double transitionVelocity = boost::get(contactOptions.at("transitionVelocity")); + if (transitionVelocity < EPS) + { + THROW_ERROR(std::invalid_argument, + "Contact option 'transitionVelocity' must be strictly positive."); + } + double stabilizationFreq = boost::get(contactOptions.at("stabilizationFreq")); + if (stabilizationFreq < 0.0) + { + THROW_ERROR(std::invalid_argument, + "Contact option 'stabilizationFreq' must be positive."); + } + + // Make sure the user-defined gravity force has the right dimension + GenericConfig worldOptions = boost::get(engineOptions.at("world")); + Eigen::VectorXd gravity = boost::get(worldOptions.at("gravity")); + if (gravity.size() != 6) + { + THROW_ERROR(std::invalid_argument, "The size of the gravity force vector must be 6."); + } + + /* Reset random number generators if setOptions is called for the first time, or if the + desired random seed has changed. */ + const VectorX & seedSeq = + boost::get>(stepperOptions.at("randomSeedSeq")); + if (!engineOptions_ || seedSeq.size() != engineOptions_->stepper.randomSeedSeq.size() || + (seedSeq.array() != engineOptions_->stepper.randomSeedSeq.array()).any()) + { + generator_.seed(std::seed_seq(seedSeq.data(), seedSeq.data() + seedSeq.size())); + } + + // Update the internal options + engineOptionsGeneric_ = engineOptions; + + // Create a fast struct accessor + engineOptions_ = std::make_unique(engineOptionsGeneric_); + + // Backup contact model as enum for fast check + contactModel_ = contactModelIt->second; + + // Set breakpoint period during the integration loop + stepperUpdatePeriod_ = updatePeriodMin; + } + + std::ptrdiff_t Engine::getRobotIndex(const std::string & robotName) const + { + auto robotIt = std::find_if(robots_.begin(), + robots_.end(), + [&robotName](const auto & robot) + { return (robot->getName() == robotName); }); + if (robotIt == robots_.end()) + { + THROW_ERROR(std::invalid_argument, + "No robot with name '", + robotName, + "' has been added to the engine."); + } + + return std::distance(robots_.begin(), robotIt); + } + + std::shared_ptr & Engine::getRobot(const std::string & robotName) + { + std::ptrdiff_t robotIndex = getRobotIndex(robotName); + return robots_[robotIndex]; + } + + const RobotState & Engine::getRobotState(const std::string & robotName) const + { + std::ptrdiff_t robotIndex = getRobotIndex(robotName); + return robotDataVec_[robotIndex].state; + } + + const StepperState & Engine::getStepperState() const + { + return stepperState_; + } + + const bool & Engine::getIsSimulationRunning() const + { + return isSimulationRunning_; + } + + double Engine::getSimulationDurationMax() + { + return TelemetryRecorder::getLogDurationMax(getTelemetryTimeUnit()); + } + + double Engine::getTelemetryTimeUnit() + { + return STEPPER_MIN_TIMESTEP; + } + + // ======================================================== + // =================== Stepper utilities ================== + // ======================================================== + + void Engine::syncStepperStateWithRobots() + { + auto qSplitIt = stepperState_.qSplit.begin(); + auto vSplitIt = stepperState_.vSplit.begin(); + auto aSplitIt = stepperState_.aSplit.begin(); + auto robotDataIt = robotDataVec_.begin(); + for (; robotDataIt != robotDataVec_.end(); + ++robotDataIt, ++qSplitIt, ++vSplitIt, ++aSplitIt) + { + *qSplitIt = robotDataIt->state.q; + *vSplitIt = robotDataIt->state.v; + *aSplitIt = robotDataIt->state.a; + } + } + + void Engine::syncRobotsStateWithStepper(bool isStateUpToDate) + { + if (isStateUpToDate) + { + auto aSplitIt = stepperState_.aSplit.begin(); + auto robotDataIt = robotDataVec_.begin(); + for (; robotDataIt != robotDataVec_.end(); ++robotDataIt, ++aSplitIt) + { + robotDataIt->state.a = *aSplitIt; + } + } + else + { + auto qSplitIt = stepperState_.qSplit.begin(); + auto vSplitIt = stepperState_.vSplit.begin(); + auto aSplitIt = stepperState_.aSplit.begin(); + auto robotDataIt = robotDataVec_.begin(); + for (; robotDataIt != robotDataVec_.end(); + ++robotDataIt, ++qSplitIt, ++vSplitIt, ++aSplitIt) + { + robotDataIt->state.q = *qSplitIt; + robotDataIt->state.v = *vSplitIt; + robotDataIt->state.a = *aSplitIt; + } + } + } + + // ======================================================== + // ================ Core physics utilities ================ + // ======================================================== + + + void Engine::computeForwardKinematics(std::shared_ptr & robot, + const Eigen::VectorXd & q, + const Eigen::VectorXd & v, + const Eigen::VectorXd & a) + { + // Create proxies for convenience + const pinocchio::Model & model = robot->pinocchioModel_; + pinocchio::Data & data = robot->pinocchioData_; + const pinocchio::GeometryModel & geomModel = robot->collisionModel_; + pinocchio::GeometryData & geomData = robot->collisionData_; + + // Update forward kinematics + pinocchio::forwardKinematics(model, data, q, v, a); + + // Update frame placements (avoiding redundant computations) + for (pinocchio::FrameIndex frameIndex = 1; + frameIndex < static_cast(model.nframes); + ++frameIndex) + { + const pinocchio::Frame & frame = model.frames[frameIndex]; + pinocchio::JointIndex parentJointIndex = frame.parent; + switch (frame.type) + { + case pinocchio::FrameType::JOINT: + /* If the frame is associated with an actual joint, no need to compute anything + new, since the frame transform is supposed to be identity. */ + data.oMf[frameIndex] = data.oMi[parentJointIndex]; + break; + case pinocchio::FrameType::BODY: + if (model.frames[frame.previousFrame].type == pinocchio::FrameType::FIXED_JOINT) + { + /* BODYs connected via FIXED_JOINT(s) have the same transform than the joint + itself, so no need to compute them twice. Here we are doing the assumption + that the previous frame transform has already been updated since it is + closer to root in kinematic tree. */ + data.oMf[frameIndex] = data.oMf[frame.previousFrame]; + } + else + { + /* BODYs connected via JOINT(s) have the identity transform, so copying parent + joint transform should be fine. */ + data.oMf[frameIndex] = data.oMi[parentJointIndex]; + } + break; + case pinocchio::FrameType::FIXED_JOINT: + case pinocchio::FrameType::SENSOR: + case pinocchio::FrameType::OP_FRAME: + default: + // Nothing special, doing the actual computation + data.oMf[frameIndex] = data.oMi[parentJointIndex] * frame.placement; + } + } + + /* Update collision information selectively, ie only for geometries involved in at least + one collision pair. */ + pinocchio::updateGeometryPlacements(model, data, geomModel, geomData); + pinocchio::computeCollisions(geomModel, geomData, false); + } + + void Engine::computeContactDynamicsAtBody(const std::shared_ptr & robot, + const pinocchio::PairIndex & collisionPairIndex, + std::shared_ptr & constraint, + pinocchio::Force & fextLocal) const + { + // TODO: It is assumed that the ground is flat. For now ground profile is not supported + // with body collision. Nevertheless it should not be to hard to generated a collision + // object simply by sampling points on the profile. + + // Define proxy for convenience + pinocchio::Data & data = robot->pinocchioData_; + + // Get the frame and joint indices + const pinocchio::GeomIndex & geometryIndex = + robot->collisionModel_.collisionPairs[collisionPairIndex].first; + pinocchio::JointIndex parentJointIndex = + robot->collisionModel_.geometryObjects[geometryIndex].parentJoint; + + // Extract collision and distance results + const hpp::fcl::CollisionResult & collisionResult = + robot->collisionData_.collisionResults[collisionPairIndex]; + + fextLocal.setZero(); + + /* There is no way to get access to the distance from the ground at this point, so it is + not possible to disable the constraint only if depth > transitionEps. */ + if (constraint) + { + constraint->disable(); + } + + for (std::size_t contactIndex = 0; contactIndex < collisionResult.numContacts(); + ++contactIndex) + { + /* Extract the contact information. + Note that there is always a single contact point while computing the collision + between two shape objects, for instance convex geometry and box primitive. */ + const auto & contact = collisionResult.getContact(contactIndex); + Eigen::Vector3d nGround = contact.normal.normalized(); // Ground normal in world frame + double depth = contact.penetration_depth; // Penetration depth (signed, negative) + pinocchio::SE3 posContactInWorld = pinocchio::SE3::Identity(); + // TODO double check that it may be between both interfaces + posContactInWorld.translation() = contact.pos; // Point inside the ground + + /* FIXME: Make sure the collision computation didn't failed. If it happens the norm of + the distance normal is not normalized (usually close to zero). If so, just assume + there is no collision at all. */ + if (nGround.norm() < 1.0 - EPS) + { + continue; + } + + // Make sure the normal is always pointing upward and the penetration depth is negative + if (nGround[2] < 0.0) + { + nGround *= -1.0; + } + if (depth > 0.0) + { + depth *= -1.0; + } + + if (contactModel_ == ContactModelType::SPRING_DAMPER) + { + // Compute the linear velocity of the contact point in world frame + const pinocchio::Motion & motionJointLocal = data.v[parentJointIndex]; + const pinocchio::SE3 & transformJointFrameInWorld = data.oMi[parentJointIndex]; + const pinocchio::SE3 transformJointFrameInContact = + posContactInWorld.actInv(transformJointFrameInWorld); + const Eigen::Vector3d vContactInWorld = + transformJointFrameInContact.act(motionJointLocal).linear(); + + // Compute the ground reaction force at contact point in world frame + const pinocchio::Force fextAtContactInGlobal = + computeContactDynamics(nGround, depth, vContactInWorld); + + // Move the force at parent frame location + fextLocal += transformJointFrameInContact.actInv(fextAtContactInGlobal); + } + else + { + // The constraint is not initialized for geometry shapes that are not supported + if (constraint) + { + // In case of slippage the contact point has actually moved and must be updated + constraint->enable(); + auto & frameConstraint = static_cast(*constraint.get()); + const pinocchio::FrameIndex frameIndex = frameConstraint.getFrameIndex(); + frameConstraint.setReferenceTransform( + {data.oMf[frameIndex].rotation(), + data.oMf[frameIndex].translation() - depth * nGround}); + frameConstraint.setNormal(nGround); + + // Only one contact constraint per collision body is supported for now + break; + } + } + } + } + + void Engine::computeContactDynamicsAtFrame( + const std::shared_ptr & robot, + pinocchio::FrameIndex frameIndex, + std::shared_ptr & constraint, + pinocchio::Force & fextLocal) const + { + /* Returns the external force in the contact frame. It must then be converted into a force + onto the parent joint. + /!\ Note that the contact dynamics depends only on kinematics data. /!\ */ + + // Define proxies for convenience + const pinocchio::Model & model = robot->pinocchioModel_; + const pinocchio::Data & data = robot->pinocchioData_; + + // Get the pose of the frame wrt the world + const pinocchio::SE3 & transformFrameInWorld = data.oMf[frameIndex]; + + // Compute the ground normal and penetration depth at the contact point + double heightGround; + Eigen::Vector3d normalGround; + const Eigen::Vector3d & posFrame = transformFrameInWorld.translation(); + engineOptions_->world.groundProfile(posFrame.head<2>(), heightGround, normalGround); + normalGround.normalize(); // Make sure the ground normal is normalized + + // First-order projection (exact assuming no curvature) + const double depth = (posFrame[2] - heightGround) * normalGround[2]; + + // Only compute the ground reaction force if the penetration depth is negative + if (depth < 0.0) + { + // Apply the force at the origin of the parent joint frame + if (contactModel_ == ContactModelType::SPRING_DAMPER) + { + // Compute the linear velocity of the contact point in world frame + const Eigen::Vector3d motionFrameLocal = + pinocchio::getFrameVelocity(model, data, frameIndex).linear(); + const Eigen::Matrix3d & rotFrame = transformFrameInWorld.rotation(); + const Eigen::Vector3d vContactInWorld = rotFrame * motionFrameLocal; + + // Compute the ground reaction force in world frame (local world aligned) + const pinocchio::Force fextAtContactInGlobal = + computeContactDynamics(normalGround, depth, vContactInWorld); + + // Deduce the ground reaction force in joint frame + fextLocal = + convertForceGlobalFrameToJoint(model, data, frameIndex, fextAtContactInGlobal); + } + else + { + // Enable fixed frame constraint + constraint->enable(); + } + } + else + { + if (contactModel_ == ContactModelType::SPRING_DAMPER) + { + // Not in contact with the ground, thus no force applied + fextLocal.setZero(); + } + else if (depth > engineOptions_->contacts.transitionEps) + { + /* Disable fixed frame constraint only if the frame move higher `transitionEps` to + avoid sporadic contact detection. */ + constraint->disable(); + } + } + + /* Move the reference position at the surface of the ground. + Note that it is must done systematically as long as the constraint is enabled because in + case of slippage the contact point has physically moved. */ + if (constraint->getIsEnabled()) + { + auto & frameConstraint = static_cast(*constraint.get()); + frameConstraint.setReferenceTransform( + {transformFrameInWorld.rotation(), posFrame - depth * normalGround}); + frameConstraint.setNormal(normalGround); + } + } + + pinocchio::Force Engine::computeContactDynamics(const Eigen::Vector3d & normalGround, + double depth, + const Eigen::Vector3d & vContactInWorld) const + { + // Initialize the contact force + Eigen::Vector3d fextInWorld; + + if (depth < 0.0) + { + // Extract some proxies + const ContactOptions & contactOptions_ = engineOptions_->contacts; + + // Compute the penetration speed + const double vDepth = vContactInWorld.dot(normalGround); + + // Compute normal force + const double fextNormal = -std::min( + contactOptions_.stiffness * depth + contactOptions_.damping * vDepth, 0.0); + fextInWorld.noalias() = fextNormal * normalGround; + + // Compute friction forces + const Eigen::Vector3d vTangential = vContactInWorld - vDepth * normalGround; + const double vRatio = + std::min(vTangential.norm() / contactOptions_.transitionVelocity, 1.0); + const double fextTangential = contactOptions_.friction * vRatio * fextNormal; + fextInWorld.noalias() -= fextTangential * vTangential; + + // Add blending factor + if (contactOptions_.transitionEps > EPS) + { + const double blendingFactor = -depth / contactOptions_.transitionEps; + const double blendingLaw = std::tanh(2.0 * blendingFactor); + fextInWorld *= blendingLaw; + } + } + else + { + fextInWorld.setZero(); + } + + return {fextInWorld, Eigen::Vector3d::Zero()}; + } + + void Engine::computeCommand(std::shared_ptr & robot, + double t, + const Eigen::VectorXd & q, + const Eigen::VectorXd & v, + Eigen::VectorXd & command) + { + // Reinitialize the external forces + command.setZero(); + + // Command the command + robot->getController()->computeCommand(t, q, v, command); + } + + template class JointModel, typename Scalar, int Options, int axis> + static std::enable_if_t< + is_pinocchio_joint_revolute_v> || + is_pinocchio_joint_revolute_unbounded_v>, + double> + getSubtreeInertiaProj(const JointModel & /* model */, + const pinocchio::Inertia & Isubtree) + { + double inertiaProj = Isubtree.inertia()(axis, axis); + for (Eigen::Index i = 0; i < 3; ++i) + { + if (i != axis) + { + inertiaProj += Isubtree.mass() * std::pow(Isubtree.lever()[i], 2); + } + } + return inertiaProj; + } + + template + static std::enable_if_t || + is_pinocchio_joint_revolute_unbounded_unaligned_v, + double> + getSubtreeInertiaProj(const JointModel & model, const pinocchio::Inertia & Isubtree) + { + return model.axis.dot(Isubtree.inertia() * model.axis) + + Isubtree.mass() * model.axis.cross(Isubtree.lever()).squaredNorm(); + } + + template + static std::enable_if_t || + is_pinocchio_joint_prismatic_unaligned_v, + double> + getSubtreeInertiaProj(const JointModel & /* model */, const pinocchio::Inertia & Isubtree) + { + return Isubtree.mass(); + } + + struct computePositionLimitsForcesAlgo : + public pinocchio::fusion::JointUnaryVisitorBase + { + typedef boost::fusion::vector< + const pinocchio::Data & /* pinocchioData */, + const Eigen::VectorXd & /* q */, + const Eigen::VectorXd & /* v */, + const Eigen::VectorXd & /* positionLimitMin */, + const Eigen::VectorXd & /* positionLimitMax */, + const std::unique_ptr & /* engineOptions */, + ContactModelType /* contactModel */, + std::shared_ptr & /* constraint */, + Eigen::VectorXd & /* u */> + ArgsType; + + template + static std::enable_if_t || + is_pinocchio_joint_revolute_unaligned_v || + is_pinocchio_joint_prismatic_v || + is_pinocchio_joint_prismatic_unaligned_v, + void> + algo(const pinocchio::JointModelBase & joint, + const pinocchio::Data & data, + const Eigen::VectorXd & q, + const Eigen::VectorXd & v, + const Eigen::VectorXd & positionLimitMin, + const Eigen::VectorXd & positionLimitMax, + const std::unique_ptr & engineOptions, + ContactModelType contactModel, + std::shared_ptr & constraint, + Eigen::VectorXd & u) + { + // Define some proxies for convenience + const pinocchio::JointIndex jointIndex = joint.id(); + const Eigen::Index positionIndex = joint.idx_q(); + const Eigen::Index velocityIndex = joint.idx_v(); + const double qJoint = q[positionIndex]; + const double qJointMin = positionLimitMin[positionIndex]; + const double qJointMax = positionLimitMax[positionIndex]; + const double vJoint = v[velocityIndex]; + const double Ia = getSubtreeInertiaProj(joint.derived(), data.Ycrb[jointIndex]); + const double stiffness = engineOptions->joints.boundStiffness; + const double damping = engineOptions->joints.boundDamping; + const double transitionEps = engineOptions->contacts.transitionEps; + + // Check if out-of-bounds + if (contactModel == ContactModelType::SPRING_DAMPER) + { + // Compute the acceleration to apply to move out of the bound + double accelJoint = 0.0; + if (qJoint > qJointMax) + { + const double qJointError = qJoint - qJointMax; + accelJoint = -std::max(stiffness * qJointError + damping * vJoint, 0.0); + } + else if (qJoint < qJointMin) + { + const double qJointError = qJoint - qJointMin; + accelJoint = -std::min(stiffness * qJointError + damping * vJoint, 0.0); + } + + // Apply the resulting force + u[velocityIndex] += Ia * accelJoint; + } + else + { + if (qJointMax < qJoint || qJoint < qJointMin) + { + // Enable fixed joint constraint + auto & jointConstraint = static_cast(*constraint.get()); + jointConstraint.setReferenceConfiguration( + Eigen::Matrix(std::clamp(qJoint, qJointMin, qJointMax))); + jointConstraint.setRotationDir(qJointMax < qJoint); + constraint->enable(); + } + else if (qJointMin + transitionEps < qJoint && qJoint < qJointMax - transitionEps) + { + // Disable fixed joint constraint + constraint->disable(); + } + } + } + + template + static std::enable_if_t || + is_pinocchio_joint_revolute_unbounded_unaligned_v, + void> + algo(const pinocchio::JointModelBase & /* joint */, + const pinocchio::Data & /* data */, + const Eigen::VectorXd & /* q */, + const Eigen::VectorXd & /* v */, + const Eigen::VectorXd & /* positionLimitMin */, + const Eigen::VectorXd & /* positionLimitMax */, + const std::unique_ptr & /* engineOptions */, + ContactModelType contactModel, + std::shared_ptr & constraint, + Eigen::VectorXd & /* u */) + { + if (contactModel == ContactModelType::CONSTRAINT) + { + // Disable fixed joint constraint + constraint->disable(); + } + } + + template + static std::enable_if_t || + is_pinocchio_joint_spherical_v || + is_pinocchio_joint_spherical_zyx_v || + is_pinocchio_joint_translation_v || + is_pinocchio_joint_planar_v || + is_pinocchio_joint_mimic_v || + is_pinocchio_joint_composite_v, + void> + algo(const pinocchio::JointModelBase & /* joint */, + const pinocchio::Data & /* data */, + const Eigen::VectorXd & /* q */, + const Eigen::VectorXd & /* v */, + const Eigen::VectorXd & /* positionLimitMin */, + const Eigen::VectorXd & /* positionLimitMax */, + const std::unique_ptr & /* engineOptions */, + ContactModelType contactModel, + std::shared_ptr & constraint, + Eigen::VectorXd & /* u */) + { + PRINT_WARNING("No position bounds implemented for this type of joint."); + if (contactModel == ContactModelType::CONSTRAINT) + { + // Disable fixed joint constraint + constraint->disable(); + } + } + }; + + struct computeVelocityLimitsForcesAlgo : + public pinocchio::fusion::JointUnaryVisitorBase + { + typedef boost::fusion::vector< + const pinocchio::Data & /* data */, + const Eigen::VectorXd & /* v */, + const Eigen::VectorXd & /* velocityLimitMax */, + const std::unique_ptr & /* engineOptions */, + ContactModelType /* contactModel */, + Eigen::VectorXd & /* u */> + ArgsType; + template + static std::enable_if_t || + is_pinocchio_joint_revolute_unaligned_v || + is_pinocchio_joint_revolute_unbounded_v || + is_pinocchio_joint_revolute_unbounded_unaligned_v || + is_pinocchio_joint_prismatic_v || + is_pinocchio_joint_prismatic_unaligned_v, + void> + algo(const pinocchio::JointModelBase & joint, + const pinocchio::Data & data, + const Eigen::VectorXd & v, + const Eigen::VectorXd & velocityLimitMax, + const std::unique_ptr & engineOptions, + ContactModelType contactModel, + Eigen::VectorXd & u) + { + // Define some proxies for convenience + const pinocchio::JointIndex jointIndex = joint.id(); + const Eigen::Index velocityIndex = joint.idx_v(); + const double vJoint = v[velocityIndex]; + const double vJointMin = -velocityLimitMax[velocityIndex]; + const double vJointMax = velocityLimitMax[velocityIndex]; + const double Ia = getSubtreeInertiaProj(joint.derived(), data.Ycrb[jointIndex]); + const double damping = engineOptions->joints.boundDamping; + + // Check if out-of-bounds + if (contactModel == ContactModelType::SPRING_DAMPER) + { + // Compute joint velocity error + double vJointError = 0.0; + if (vJoint > vJointMax) + { + vJointError = vJoint - vJointMax; + } + else if (vJoint < vJointMin) + { + vJointError = vJoint - vJointMin; + } + else + { + return; + } + + // Generate acceleration in the opposite direction if out-of-bounds + const double accelJoint = -2.0 * damping * vJointError; + + // Apply the resulting force + u[velocityIndex] += Ia * accelJoint; + } + } + + template + static std::enable_if_t || + is_pinocchio_joint_spherical_v || + is_pinocchio_joint_spherical_zyx_v || + is_pinocchio_joint_translation_v || + is_pinocchio_joint_planar_v || + is_pinocchio_joint_mimic_v || + is_pinocchio_joint_composite_v, + void> + algo(const pinocchio::JointModelBase & /* joint */, + const pinocchio::Data & /* data */, + const Eigen::VectorXd & /* v */, + const Eigen::VectorXd & /* velocityLimitMax */, + const std::unique_ptr & /* engineOptions */, + ContactModelType /* contactModel */, + Eigen::VectorXd & /* u */) + { + PRINT_WARNING("No velocity bounds implemented for this type of joint."); + } + }; + + void Engine::computeInternalDynamics(const std::shared_ptr & robot, + RobotData & robotData, + double /* t */, + const Eigen::VectorXd & q, + const Eigen::VectorXd & v, + Eigen::VectorXd & uInternal) const + { + // Define some proxies + const pinocchio::Model & model = robot->pinocchioModel_; + const pinocchio::Data & data = robot->pinocchioData_; + + // Enforce the position limit (rigid joints only) + if (robot->modelOptions_->joints.enablePositionLimit) + { + const Eigen::VectorXd & positionLimitMin = robot->getPositionLimitMin(); + const Eigen::VectorXd & positionLimitMax = robot->getPositionLimitMax(); + const std::vector & rigidJointIndices = + robot->getRigidJointIndices(); + for (std::size_t i = 0; i < rigidJointIndices.size(); ++i) + { + auto & constraint = robotData.constraints.boundJoints[i].second; + computePositionLimitsForcesAlgo::run( + model.joints[rigidJointIndices[i]], + typename computePositionLimitsForcesAlgo::ArgsType(data, + q, + v, + positionLimitMin, + positionLimitMax, + engineOptions_, + contactModel_, + constraint, + uInternal)); + } + } + + // Enforce the velocity limit (rigid joints only) + if (robot->modelOptions_->joints.enableVelocityLimit) + { + const Eigen::VectorXd & velocityLimitMax = robot->getVelocityLimit(); + for (pinocchio::JointIndex rigidJointIndex : robot->getRigidJointIndices()) + { + computeVelocityLimitsForcesAlgo::run( + model.joints[rigidJointIndex], + typename computeVelocityLimitsForcesAlgo::ArgsType( + data, v, velocityLimitMax, engineOptions_, contactModel_, uInternal)); + } + } + + // Compute the flexibilities (only support JointModelType::SPHERICAL so far) + double angle; + Eigen::Matrix3d rotJlog3; + const Robot::DynamicsOptions & modelDynOptions = robot->modelOptions_->dynamics; + const std::vector & flexibilityJointIndices = + robot->getFlexibleJointIndices(); + for (std::size_t i = 0; i < flexibilityJointIndices.size(); ++i) + { + const pinocchio::JointIndex jointIndex = flexibilityJointIndices[i]; + const Eigen::Index positionIndex = model.joints[jointIndex].idx_q(); + const Eigen::Index velocityIndex = model.joints[jointIndex].idx_v(); + const Eigen::Vector3d & stiffness = modelDynOptions.flexibilityConfig[i].stiffness; + const Eigen::Vector3d & damping = modelDynOptions.flexibilityConfig[i].damping; + + const Eigen::Map quat(q.segment<4>(positionIndex).data()); + const Eigen::Vector3d angleAxis = pinocchio::quaternion::log3(quat, angle); + if (angle > 0.95 * M_PI) // Angle is always positive + { + THROW_ERROR(std::runtime_error, + "Flexible joint angle must be smaller than 0.95 * pi."); + } + pinocchio::Jlog3(angle, angleAxis, rotJlog3); + uInternal.segment<3>(velocityIndex) -= + rotJlog3 * (stiffness.array() * angleAxis.array()).matrix(); + uInternal.segment<3>(velocityIndex).array() -= + damping.array() * v.segment<3>(velocityIndex).array(); + } + } + + void Engine::computeCollisionForces(const std::shared_ptr & robot, + RobotData & robotData, + ForceVector & fext, + bool isStateUpToDate) const + { + // Compute the forces at contact points + const std::vector & contactFrameIndices = + robot->getContactFrameIndices(); + for (std::size_t i = 0; i < contactFrameIndices.size(); ++i) + { + // Compute force at the given contact frame. + const pinocchio::FrameIndex frameIndex = contactFrameIndices[i]; + auto & constraint = robotData.constraints.contactFrames[i].second; + pinocchio::Force & fextLocal = robotData.contactFrameForces[i]; + if (!isStateUpToDate) + { + computeContactDynamicsAtFrame(robot, frameIndex, constraint, fextLocal); + } + + // Apply the force at the origin of the parent joint frame, in local joint frame + const pinocchio::JointIndex parentJointIndex = + robot->pinocchioModel_.frames[frameIndex].parent; + fext[parentJointIndex] += fextLocal; + + // Convert contact force from global frame to local frame to store it in contactForces_ + const pinocchio::SE3 & transformContactInJoint = + robot->pinocchioModel_.frames[frameIndex].placement; + robot->contactForces_[i] = transformContactInJoint.actInv(fextLocal); + } + + // Compute the force at collision bodies + const std::vector & collisionBodyIndices = + robot->getCollisionBodyIndices(); + const std::vector> & collisionPairIndices = + robot->getCollisionPairIndices(); + for (std::size_t i = 0; i < collisionBodyIndices.size(); ++i) + { + /* Compute force at the given collision body. + It returns the force applied at the origin of parent joint frame in global frame. */ + const pinocchio::FrameIndex frameIndex = collisionBodyIndices[i]; + const pinocchio::JointIndex parentJointIndex = + robot->pinocchioModel_.frames[frameIndex].parent; + for (std::size_t j = 0; j < collisionPairIndices[i].size(); ++j) + { + pinocchio::Force & fextLocal = robotData.collisionBodiesForces[i][j]; + if (!isStateUpToDate) + { + const pinocchio::PairIndex & collisionPairIndex = collisionPairIndices[i][j]; + auto & constraint = robotData.constraints.collisionBodies[i][j].second; + computeContactDynamicsAtBody(robot, collisionPairIndex, constraint, fextLocal); + } + + // Apply the force at the origin of the parent joint frame, in local joint frame + fext[parentJointIndex] += fextLocal; + } + } + } + + void Engine::computeExternalForces(const std::shared_ptr & robot, + RobotData & robotData, + double t, + const Eigen::VectorXd & q, + const Eigen::VectorXd & v, + ForceVector & fext) + { + // Add the effect of user-defined external impulse forces + auto isImpulseForceActiveIt = robotData.isImpulseForceActiveVec.begin(); + auto impulseForceIt = robotData.impulseForces.begin(); + for (; impulseForceIt != robotData.impulseForces.end(); + ++isImpulseForceActiveIt, ++impulseForceIt) + { + /* Do not check if the force is active at this point. This is managed at stepper level + to get around the ambiguous t- versus t+. */ + if (*isImpulseForceActiveIt) + { + const pinocchio::FrameIndex frameIndex = impulseForceIt->frameIndex; + const pinocchio::JointIndex parentJointIndex = + robot->pinocchioModel_.frames[frameIndex].parent; + fext[parentJointIndex] += convertForceGlobalFrameToJoint(robot->pinocchioModel_, + robot->pinocchioData_, + frameIndex, + impulseForceIt->force); + } + } + + // Add the effect of time-continuous external force profiles + for (auto & profileForce : robotData.profileForces) + { + const pinocchio::FrameIndex frameIndex = profileForce.frameIndex; + const pinocchio::JointIndex parentJointIndex = + robot->pinocchioModel_.frames[frameIndex].parent; + if (profileForce.updatePeriod < EPS) + { + profileForce.force = profileForce.func(t, q, v); + } + fext[parentJointIndex] += convertForceGlobalFrameToJoint( + robot->pinocchioModel_, robot->pinocchioData_, frameIndex, profileForce.force); + } + } + + void Engine::computeCouplingForces(double t, + const std::vector & qSplit, + const std::vector & vSplit) + { + for (auto & couplingForce : couplingForces_) + { + // Extract info about the first robot involved + const std::ptrdiff_t robotIndex1 = couplingForce.robotIndex1; + const std::shared_ptr & robot1 = robots_[robotIndex1]; + const Eigen::VectorXd & q1 = qSplit[robotIndex1]; + const Eigen::VectorXd & v1 = vSplit[robotIndex1]; + const pinocchio::FrameIndex frameIndex1 = couplingForce.frameIndex1; + ForceVector & fext1 = robotDataVec_[robotIndex1].state.fExternal; + + // Extract info about the second robot involved + const std::ptrdiff_t robotIndex2 = couplingForce.robotIndex2; + const std::shared_ptr & robot2 = robots_[robotIndex2]; + const Eigen::VectorXd & q2 = qSplit[robotIndex2]; + const Eigen::VectorXd & v2 = vSplit[robotIndex2]; + const pinocchio::FrameIndex frameIndex2 = couplingForce.frameIndex2; + ForceVector & fext2 = robotDataVec_[robotIndex2].state.fExternal; + + // Compute the coupling force + pinocchio::Force force = couplingForce.func(t, q1, v1, q2, v2); + const pinocchio::JointIndex parentJointIndex1 = + robot1->pinocchioModel_.frames[frameIndex1].parent; + fext1[parentJointIndex1] += convertForceGlobalFrameToJoint( + robot1->pinocchioModel_, robot1->pinocchioData_, frameIndex1, force); + + // Move force from frame1 to frame2 to apply it to the second robot + force.toVector() *= -1; + const pinocchio::JointIndex parentJointIndex2 = + robot2->pinocchioModel_.frames[frameIndex2].parent; + const Eigen::Vector3d offset = robot2->pinocchioData_.oMf[frameIndex2].translation() - + robot1->pinocchioData_.oMf[frameIndex1].translation(); + force.angular() -= offset.cross(force.linear()); + fext2[parentJointIndex2] += convertForceGlobalFrameToJoint( + robot2->pinocchioModel_, robot2->pinocchioData_, frameIndex2, force); + } + } + + void Engine::computeAllTerms(double t, + const std::vector & qSplit, + const std::vector & vSplit, + bool isStateUpToDate) + { + // Reinitialize the external forces and internal efforts + for (auto & robotData : robotDataVec_) + { + for (pinocchio::Force & fext_i : robotData.state.fExternal) + { + fext_i.setZero(); + } + if (!isStateUpToDate) + { + robotData.state.uInternal.setZero(); + } + } + + // Compute the internal forces + computeCouplingForces(t, qSplit, vSplit); + + // Compute each individual robot dynamics + auto robotIt = robots_.begin(); + auto robotDataIt = robotDataVec_.begin(); + auto qIt = qSplit.begin(); + auto vIt = vSplit.begin(); + for (; robotIt != robots_.end(); ++robotIt, ++robotDataIt, ++qIt, ++vIt) + { + // Define some proxies + ForceVector & fext = robotDataIt->state.fExternal; + Eigen::VectorXd & uInternal = robotDataIt->state.uInternal; + + /* Compute internal dynamics, namely the efforts in joint space associated with + position/velocity bounds dynamics, and flexibility dynamics. */ + if (!isStateUpToDate) + { + computeInternalDynamics(*robotIt, *robotDataIt, t, *qIt, *vIt, uInternal); + } + + /* Compute the collision forces and estimated time at which the contact state will + changed (Take-off / Touch-down). */ + computeCollisionForces(*robotIt, *robotDataIt, fext, isStateUpToDate); + + // Compute the external contact forces. + computeExternalForces(*robotIt, *robotDataIt, t, *qIt, *vIt, fext); + } + } + + void Engine::computeRobotsDynamics(double t, + const std::vector & qSplit, + const std::vector & vSplit, + std::vector & aSplit, + bool isStateUpToDate) + { + /* Note that the position of the free flyer is in world frame, whereas the velocities and + accelerations are relative to the parent body frame. */ + + // Make sure that a simulation is running + if (!isSimulationRunning_) + { + THROW_ERROR(std::logic_error, + "No simulation running. Please start one before calling this method."); + } + + // Make sure memory has been allocated for the output acceleration + aSplit.resize(vSplit.size()); + + if (!isStateUpToDate) + { + // Update kinematics for each robot + auto robotIt = robots_.begin(); + auto robotDataIt = robotDataVec_.begin(); + auto qIt = qSplit.begin(); + auto vIt = vSplit.begin(); + for (; robotIt != robots_.end(); ++robotIt, ++robotDataIt, ++qIt, ++vIt) + { + const Eigen::VectorXd & aPrev = robotDataIt->statePrev.a; + computeForwardKinematics(*robotIt, *qIt, *vIt, aPrev); + } + } + + /* Compute internal and external forces and efforts applied on every robots, excluding + user-specified internal dynamics if any. + + Note that one must call this method BEFORE updating the sensors since the force sensor + measurements rely on robot_->contactForces_. */ + computeAllTerms(t, qSplit, vSplit, isStateUpToDate); + + // Compute each individual robot dynamics + auto robotIt = robots_.begin(); + auto robotDataIt = robotDataVec_.begin(); + auto qIt = qSplit.begin(); + auto vIt = vSplit.begin(); + auto contactForcesPrevIt = contactForcesPrev_.begin(); + auto fPrevIt = fPrev_.begin(); + auto aPrevIt = aPrev_.begin(); + auto aIt = aSplit.begin(); + for (; robotIt != robots_.end(); ++robotIt, + ++robotDataIt, + ++qIt, + ++vIt, + ++aIt, + ++contactForcesPrevIt, + ++fPrevIt, + ++aPrevIt) + { + // Define some proxies + Eigen::VectorXd & u = robotDataIt->state.u; + Eigen::VectorXd & command = robotDataIt->state.command; + Eigen::VectorXd & uMotor = robotDataIt->state.uMotor; + Eigen::VectorXd & uInternal = robotDataIt->state.uInternal; + Eigen::VectorXd & uCustom = robotDataIt->state.uCustom; + ForceVector & fext = robotDataIt->state.fExternal; + const Eigen::VectorXd & aPrev = robotDataIt->statePrev.a; + const Eigen::VectorXd & uMotorPrev = robotDataIt->statePrev.uMotor; + const ForceVector & fextPrev = robotDataIt->statePrev.fExternal; + + /* Update the sensor data if necessary (only for infinite update frequency). + Note that it is impossible to have access to the current accelerations and efforts + since they depend on the sensor values themselves. */ + if (!isStateUpToDate && engineOptions_->stepper.sensorsUpdatePeriod < EPS) + { + // Roll back to forces and accelerations computed at previous iteration + contactForcesPrevIt->swap((*robotIt)->contactForces_); + fPrevIt->swap((*robotIt)->pinocchioData_.f); + aPrevIt->swap((*robotIt)->pinocchioData_.a); + + // Update sensors based on previous accelerations and forces + (*robotIt)->computeSensorMeasurements(t, *qIt, *vIt, aPrev, uMotorPrev, fextPrev); + + // Restore current forces and accelerations + contactForcesPrevIt->swap((*robotIt)->contactForces_); + fPrevIt->swap((*robotIt)->pinocchioData_.f); + aPrevIt->swap((*robotIt)->pinocchioData_.a); + } + + /* Update the controller command if necessary (only for infinite update frequency). + Make sure that the sensor state has been updated beforehand. */ + if (engineOptions_->stepper.controllerUpdatePeriod < EPS) + { + computeCommand(*robotIt, t, *qIt, *vIt, command); + } + + /* Compute the actual motor effort. + Note that it is impossible to have access to the current accelerations. */ + (*robotIt)->computeMotorEfforts(t, *qIt, *vIt, aPrev, command); + uMotor = (*robotIt)->getMotorEfforts(); + + /* Compute the user-defined internal dynamics. + Make sure that the sensor state has been updated beforehand since the user-defined + internal dynamics may rely on it. */ + uCustom.setZero(); + (*robotIt)->getController()->internalDynamics(t, *qIt, *vIt, uCustom); + + // Compute the total effort vector + u = uInternal + uCustom; + for (const auto & motor : (*robotIt)->getMotors()) + { + const std::size_t motorIndex = motor->getIndex(); + const Eigen::Index motorVelocityIndex = motor->getJointVelocityIndex(); + u[motorVelocityIndex] += uMotor[motorIndex]; + } + + // Compute the dynamics + *aIt = + computeAcceleration(*robotIt, *robotDataIt, *qIt, *vIt, u, fext, isStateUpToDate); + } + } + + const Eigen::VectorXd & Engine::computeAcceleration(std::shared_ptr & robot, + RobotData & robotData, + const Eigen::VectorXd & q, + const Eigen::VectorXd & v, + const Eigen::VectorXd & u, + ForceVector & fext, + bool isStateUpToDate, + bool ignoreBounds) + { + const pinocchio::Model & model = robot->pinocchioModel_; + pinocchio::Data & data = robot->pinocchioData_; + + if (robot->hasConstraints()) + { + if (!isStateUpToDate) + { + // Compute kinematic constraints. It will take care of updating the joint Jacobian. + robot->computeConstraints(q, v); + + // Compute non-linear effects + pinocchio::nonLinearEffects(model, data, q, v); + } + + // Project external forces from cartesian space to joint space. + data.u = u; + for (int i = 1; i < model.njoints; ++i) + { + /* Skip computation if force is zero for efficiency. It should be the case most + often. */ + if ((fext[i].toVector().array().abs() > EPS).any()) + { + if (!isStateUpToDate) + { + pinocchio::getJointJacobian( + model, data, i, pinocchio::LOCAL, robotData.jointJacobians[i]); + } + data.u.noalias() += + robotData.jointJacobians[i].transpose() * fext[i].toVector(); + } + } + + // Call forward dynamics + bool isSucess = robotData.constraintSolver->SolveBoxedForwardDynamics( + engineOptions_->constraints.regularization, isStateUpToDate, ignoreBounds); + + /* Monitor number of successive constraint solving failure. Exception raising is + delegated to the 'step' method since this method is supposed to always succeed. */ + if (isSucess) + { + robotData.successiveSolveFailed = 0U; + } + else + { + if (engineOptions_->stepper.verbose) + { + std::cout << "Constraint solver failure." << std::endl; + } + ++robotData.successiveSolveFailed; + } + + // Restore contact frame forces and bounds internal efforts + robotData.constraints.foreach( + ConstraintNodeType::BOUNDS_JOINTS, + [&u = robotData.state.u, + &uInternal = robotData.state.uInternal, + &joints = const_cast(model.joints)]( + std::shared_ptr & constraint, + ConstraintNodeType /* node */) + { + if (!constraint->getIsEnabled()) + { + return; + } + + Eigen::VectorXd & uJoint = constraint->lambda_; + const auto & jointConstraint = + static_cast(*constraint.get()); + const auto & jointModel = joints[jointConstraint.getJointIndex()]; + jointModel.jointVelocitySelector(uInternal) += uJoint; + jointModel.jointVelocitySelector(u) += uJoint; + }); + + auto constraintIt = robotData.constraints.contactFrames.begin(); + auto forceIt = robot->contactForces_.begin(); + for (; constraintIt != robotData.constraints.contactFrames.end(); + ++constraintIt, ++forceIt) + { + auto & constraint = *constraintIt->second.get(); + if (!constraint.getIsEnabled()) + { + continue; + } + const auto & frameConstraint = static_cast(constraint); + + // Extract force in local reference-frame-aligned from lagrangian multipliers + pinocchio::Force fextInLocal(frameConstraint.lambda_.head<3>(), + frameConstraint.lambda_[3] * + Eigen::Vector3d::UnitZ()); + + // Compute force in local world aligned frame + const Eigen::Matrix3d & rotationLocal = frameConstraint.getLocalFrame(); + const pinocchio::Force fextInWorld({ + rotationLocal * fextInLocal.linear(), + rotationLocal * fextInLocal.angular(), + }); + + // Convert the force from local world aligned frame to local frame + const pinocchio::FrameIndex frameIndex = frameConstraint.getFrameIndex(); + const auto rotationWorldInContact = data.oMf[frameIndex].rotation().transpose(); + forceIt->linear().noalias() = rotationWorldInContact * fextInWorld.linear(); + forceIt->angular().noalias() = rotationWorldInContact * fextInWorld.angular(); + + // Convert the force from local world aligned to local parent joint + pinocchio::JointIndex parentJointIndex = model.frames[frameIndex].parent; + fext[parentJointIndex] += + convertForceGlobalFrameToJoint(model, data, frameIndex, fextInWorld); + } + + robotData.constraints.foreach( + ConstraintNodeType::COLLISION_BODIES, + [&fext, &model, &data](std::shared_ptr & constraint, + ConstraintNodeType /* node */) + { + if (!constraint->getIsEnabled()) + { + return; + } + const auto & frameConstraint = + static_cast(*constraint.get()); + + // Extract force in world frame from lagrangian multipliers + pinocchio::Force fextInLocal(frameConstraint.lambda_.head<3>(), + frameConstraint.lambda_[3] * + Eigen::Vector3d::UnitZ()); + + // Compute force in world frame + const Eigen::Matrix3d & rotationLocal = frameConstraint.getLocalFrame(); + const pinocchio::Force fextInWorld({ + rotationLocal * fextInLocal.linear(), + rotationLocal * fextInLocal.angular(), + }); + + // Convert the force from local world aligned to local parent joint + const pinocchio::FrameIndex frameIndex = frameConstraint.getFrameIndex(); + const pinocchio::JointIndex parentJointIndex = model.frames[frameIndex].parent; + fext[parentJointIndex] += + convertForceGlobalFrameToJoint(model, data, frameIndex, fextInWorld); + }); + + return data.ddq; + } + else + { + // No kinematic constraint: run aba algorithm + return pinocchio_overload::aba(model, data, q, v, u, fext); + } + } + + // =================================================================== + // ================ Log reading and writing utilities ================ + // =================================================================== + + std::shared_ptr Engine::getLog() + { + // Update internal log data buffer if uninitialized + if (!logData_) + { + logData_ = std::make_shared(telemetryRecorder_->getLog()); + } + + // Return shared pointer to internal log data buffer + return std::const_pointer_cast(logData_); + } + + LogData readLogHdf5(const std::string & filename) + { + LogData logData{}; + + // Open HDF5 logfile + std::unique_ptr file; + try + { + /* Specifying `H5F_CLOSE_STRONG` is necessary to completely close the file (including + all open objects) before returning. See: + https://docs.hdfgroup.org/hdf5/v1_12/group___f_a_p_l.html#ga60e3567f677fd3ade75b909b636d7b9c + */ + H5::FileAccPropList access_plist; + access_plist.setFcloseDegree(H5F_CLOSE_STRONG); + file = std::make_unique( + filename, H5F_ACC_RDONLY, H5::FileCreatPropList::DEFAULT, access_plist); + } + catch (const H5::FileIException &) + { + THROW_ERROR(std::runtime_error, + "Impossible to open the log file. Make sure it exists and " + "you have reading permissions."); + } + + // Extract all constants. There is no ordering among them, unlike variables. + H5::Group constantsGroup = file->openGroup("/constants"); + file->iterateElems( + "/constants", + NULL, + [](hid_t group, const char * name, void * op_data) -> herr_t + { + LogData * logDataPtr = static_cast(op_data); + H5::Group _constantsGroup(group); + const H5::DataSet constantDataSet = _constantsGroup.openDataSet(name); + const H5::DataSpace constantSpace = H5::DataSpace(H5S_SCALAR); + const H5::StrType constantDataType = constantDataSet.getStrType(); + const hssize_t numBytes = constantDataType.getSize(); + H5::StrType stringType(H5::PredType::C_S1, numBytes); + stringType.setStrpad(H5T_str_t::H5T_STR_NULLPAD); + std::string value(numBytes, '\0'); + constantDataSet.read(value.data(), stringType, constantSpace); + logDataPtr->constants.emplace_back(name, std::move(value)); + return 0; + }, + static_cast(&logData)); + + // Extract the times + const H5::DataSet globalTimeDataSet = file->openDataSet(std::string{GLOBAL_TIME}); + const H5::DataSpace timeSpace = globalTimeDataSet.getSpace(); + const hssize_t numData = timeSpace.getSimpleExtentNpoints(); + logData.times.resize(numData); + globalTimeDataSet.read(logData.times.data(), H5::PredType::NATIVE_INT64); + + // Add "unit" attribute to GLOBAL_TIME vector + const H5::Attribute unitAttrib = globalTimeDataSet.openAttribute("unit"); + unitAttrib.read(H5::PredType::NATIVE_DOUBLE, &logData.timeUnit); + + // Get the (partitioned) number of variables + H5::Group variablesGroup = file->openGroup("/variables"); + int64_t numInt = 0, numFloat = 0; + std::pair numVar{numInt, numFloat}; + H5Literate( + variablesGroup.getId(), + H5_INDEX_CRT_ORDER, + H5_ITER_INC, + NULL, + [](hid_t group, const char * name, const H5L_info_t * /* oinfo */, void * op_data) + -> herr_t + { + auto & [_numInt, _numFloat] = + *static_cast *>(op_data); + H5::Group fieldGroup = H5::Group(group).openGroup(name); + const H5::DataSet valueDataset = fieldGroup.openDataSet("value"); + const H5T_class_t valueType = valueDataset.getTypeClass(); + if (valueType == H5T_FLOAT) + { + ++_numFloat; + } + else + { + ++_numInt; + } + return 0; + }, + static_cast(&numVar)); + + // Pre-allocate memory + logData.integerValues.resize(numInt, numData); + logData.floatValues.resize(numFloat, numData); + VectorX intVector(numData); + VectorX floatVector(numData); + logData.variableNames.reserve(1 + numInt + numFloat); + logData.variableNames.emplace_back(GLOBAL_TIME); + + // Read all variables while preserving ordering + using opDataT = std::tuple &, VectorX &>; + opDataT opData{logData, intVector, floatVector}; + H5Literate( + variablesGroup.getId(), + H5_INDEX_CRT_ORDER, + H5_ITER_INC, + NULL, + [](hid_t group, const char * name, const H5L_info_t * /* oinfo */, void * op_data) + -> herr_t + { + auto & [logDataIn, intVectorIn, floatVectorIn] = *static_cast(op_data); + const Eigen::Index varIndex = logDataIn.variableNames.size() - 1; + const int64_t numIntIn = logDataIn.integerValues.rows(); + H5::Group fieldGroup = H5::Group(group).openGroup(name); + const H5::DataSet valueDataset = fieldGroup.openDataSet("value"); + if (varIndex < numIntIn) + { + valueDataset.read(intVectorIn.data(), H5::PredType::NATIVE_INT64); + logDataIn.integerValues.row(varIndex) = intVectorIn; + } + else + { + valueDataset.read(floatVectorIn.data(), H5::PredType::NATIVE_DOUBLE); + logDataIn.floatValues.row(varIndex - numIntIn) = floatVectorIn; + } + logDataIn.variableNames.push_back(name); + return 0; + }, + static_cast(&opData)); + + // Close file once done + file->close(); + + return logData; + } + + LogData Engine::readLog(const std::string & filename, const std::string & format) + { + if (format == "binary") + { + return TelemetryRecorder::readLog(filename); + } + if (format == "hdf5") + { + return readLogHdf5(filename); + } + THROW_ERROR(std::invalid_argument, + "Format '", + format, + "' not recognized. It must be either 'binary' or 'hdf5'."); + } + + void writeLogHdf5(const std::string & filename, const std::shared_ptr & logData) + { + // Open HDF5 logfile + std::unique_ptr file; + try + { + H5::FileAccPropList access_plist; + access_plist.setFcloseDegree(H5F_CLOSE_STRONG); + file = std::make_unique( + filename, H5F_ACC_TRUNC, H5::FileCreatPropList::DEFAULT, access_plist); + } + catch (const H5::FileIException & open_file) + { + THROW_ERROR(std::runtime_error, + "Impossible to create the log file. Make sure the root folder " + "exists and you have writing permissions."); + } + + // Add "VERSION" attribute + const H5::DataSpace versionSpace = H5::DataSpace(H5S_SCALAR); + const H5::Attribute versionAttrib = + file->createAttribute("VERSION", H5::PredType::NATIVE_INT32, versionSpace); + versionAttrib.write(H5::PredType::NATIVE_INT32, &logData->version); + + // Add "START_TIME" attribute + int64_t time = std::time(nullptr); + const H5::DataSpace startTimeSpace = H5::DataSpace(H5S_SCALAR); + const H5::Attribute startTimeAttrib = + file->createAttribute("START_TIME", H5::PredType::NATIVE_INT64, startTimeSpace); + startTimeAttrib.write(H5::PredType::NATIVE_INT64, &time); + + // Add GLOBAL_TIME vector + const hsize_t timeDims[1] = {hsize_t(logData->times.size())}; + const H5::DataSpace globalTimeSpace = H5::DataSpace(1, timeDims); + const H5::DataSet globalTimeDataSet = file->createDataSet( + std::string{GLOBAL_TIME}, H5::PredType::NATIVE_INT64, globalTimeSpace); + globalTimeDataSet.write(logData->times.data(), H5::PredType::NATIVE_INT64); + + // Add "unit" attribute to GLOBAL_TIME vector + const H5::DataSpace unitSpace = H5::DataSpace(H5S_SCALAR); + const H5::Attribute unitAttrib = + globalTimeDataSet.createAttribute("unit", H5::PredType::NATIVE_DOUBLE, unitSpace); + unitAttrib.write(H5::PredType::NATIVE_DOUBLE, &logData->timeUnit); + + // Add group "constants" + H5::Group constantsGroup(file->createGroup("constants")); + for (const auto & [key, value] : logData->constants) + { + // Define a dataset with a single string of fixed length + const H5::DataSpace constantSpace = H5::DataSpace(H5S_SCALAR); + H5::StrType stringType(H5::PredType::C_S1, std::max(value.size(), std::size_t(1))); + + // To tell parser continue reading if '\0' is encountered + stringType.setStrpad(H5T_str_t::H5T_STR_NULLPAD); + + // Write the constant + H5::DataSet constantDataSet = + constantsGroup.createDataSet(key, stringType, constantSpace); + constantDataSet.write(value, stringType); + } + + // Temporary contiguous storage for variables + VectorX intVector; + VectorX floatVector; + + // Get the number of integer and float variables + const Eigen::Index numInt = logData->integerValues.rows(); + const Eigen::Index numFloat = logData->floatValues.rows(); + + /* Add group "variables". + C++ helper `file->createGroup("variables")` cannot be used + because we want to preserve order. */ + hid_t group_creation_plist = H5Pcreate(H5P_GROUP_CREATE); + H5Pset_link_creation_order(group_creation_plist, + H5P_CRT_ORDER_TRACKED | H5P_CRT_ORDER_INDEXED); + hid_t group_id = H5Gcreate( + file->getId(), "/variables/", H5P_DEFAULT, group_creation_plist, H5P_DEFAULT); + H5::Group variablesGroup(group_id); + + // Store all integers + for (Eigen::Index i = 0; i < numInt; ++i) + { + const std::string & key = logData->variableNames[i]; + + // Create group for field + H5::Group fieldGroup = variablesGroup.createGroup(key); + + // Enable compression and shuffling + H5::DSetCreatPropList plist; + const hsize_t chunkSize[1] = {std::max(hsize_t(1), hsize_t(logData->times.size()))}; + plist.setChunk(1, chunkSize); // Read the whole vector at once. + plist.setShuffle(); + plist.setDeflate(4); + + // Create time dataset using symbolic link + fieldGroup.link(H5L_TYPE_HARD, toString("/", GLOBAL_TIME), "time"); + + // Create variable dataset + H5::DataSpace valueSpace = H5::DataSpace(1, timeDims); + H5::DataSet valueDataset = + fieldGroup.createDataSet("value", H5::PredType::NATIVE_INT64, valueSpace, plist); + + // Write values in one-shot for efficiency + intVector = logData->integerValues.row(i); + valueDataset.write(intVector.data(), H5::PredType::NATIVE_INT64); + } + + // Store all floats + for (Eigen::Index i = 0; i < numFloat; ++i) + { + const std::string & key = logData->variableNames[i + 1 + numInt]; + + // Create group for field + H5::Group fieldGroup(variablesGroup.createGroup(key)); + + // Enable compression and shuffling + H5::DSetCreatPropList plist; + const hsize_t chunkSize[1] = {std::max(hsize_t(1), hsize_t(logData->times.size()))}; + plist.setChunk(1, chunkSize); // Read the whole vector at once. + plist.setShuffle(); + plist.setDeflate(4); + + // Create time dataset using symbolic link + fieldGroup.link(H5L_TYPE_HARD, toString("/", GLOBAL_TIME), "time"); + + // Create variable dataset + H5::DataSpace valueSpace = H5::DataSpace(1, timeDims); + H5::DataSet valueDataset = + fieldGroup.createDataSet("value", H5::PredType::NATIVE_DOUBLE, valueSpace, plist); + + // Write values + floatVector = logData->floatValues.row(i); + valueDataset.write(floatVector.data(), H5::PredType::NATIVE_DOUBLE); + } + + // Close file once done + file->close(); + } + + void Engine::writeLog(const std::string & filename, const std::string & format) + { + // Make sure there is something to write + if (!isTelemetryConfigured_) + { + THROW_ERROR(bad_control_flow, + "Telemetry not configured. Please start a simulation before writing log."); + } + + // Pick the appropriate format + if (format == "binary") + { + telemetryRecorder_->writeLog(filename); + } + else if (format == "hdf5") + { + // Extract log data + std::shared_ptr logData = getLog(); + + // Write log data + writeLogHdf5(filename, logData); + } + else { - THROW_ERROR(bad_control_flow, "Engine not initialized."); + THROW_ERROR(std::invalid_argument, + "Format '", + format, + "' not recognized. It must be either 'binary' or 'hdf5'."); } - return EngineMultiRobot::getSystemState(""); } } diff --git a/core/src/engine/engine_multi_robot.cc b/core/src/engine/engine_multi_robot.cc deleted file mode 100644 index a76ed8a53..000000000 --- a/core/src/engine/engine_multi_robot.cc +++ /dev/null @@ -1,4239 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#include "pinocchio/parsers/urdf.hpp" -#include "pinocchio/spatial/inertia.hpp" // `pinocchio::Inertia` -#include "pinocchio/spatial/force.hpp" // `pinocchio::Force` -#include "pinocchio/spatial/se3.hpp" // `pinocchio::SE3` -#include "pinocchio/spatial/explog.hpp" // `pinocchio::exp3`, `pinocchio::log3` -#include "pinocchio/spatial/explog-quaternion.hpp" // `pinocchio::quaternion::log3` -#include "pinocchio/multibody/visitor.hpp" // `pinocchio::fusion::JointUnaryVisitorBase` -#include "pinocchio/multibody/joint/joint-model-base.hpp" // `pinocchio::JointModelBase` -#include "pinocchio/algorithm/center-of-mass.hpp" // `pinocchio::getComFromCrba` -#include "pinocchio/algorithm/frames.hpp" // `pinocchio::getFrameVelocity` -#include "pinocchio/algorithm/jacobian.hpp" // `pinocchio::getJointJacobian` -#include "pinocchio/algorithm/energy.hpp" // `pinocchio::computePotentialEnergy` -#include "pinocchio/algorithm/joint-configuration.hpp" // `pinocchio::normalize` -#include "pinocchio/algorithm/geometry.hpp" // `pinocchio::computeCollisions` - -#include "H5Cpp.h" -#include "json/json.h" - -#include "jiminy/core/telemetry/fwd.h" -#include "jiminy/core/hardware/fwd.h" -#include "jiminy/core/utilities/pinocchio.h" -#include "jiminy/core/utilities/json.h" -#include "jiminy/core/io/file_device.h" -#include "jiminy/core/io/serialization.h" -#include "jiminy/core/telemetry/telemetry_sender.h" -#include "jiminy/core/telemetry/telemetry_data.h" -#include "jiminy/core/telemetry/telemetry_recorder.h" -#include "jiminy/core/constraints/abstract_constraint.h" -#include "jiminy/core/constraints/joint_constraint.h" -#include "jiminy/core/constraints/frame_constraint.h" -#include "jiminy/core/hardware/abstract_motor.h" -#include "jiminy/core/hardware/abstract_sensor.h" -#include "jiminy/core/robot/robot.h" -#include "jiminy/core/robot/pinocchio_overload_algorithms.h" -#include "jiminy/core/control/abstract_controller.h" -#include "jiminy/core/control/controller_functor.h" -#include "jiminy/core/solver/constraint_solvers.h" -#include "jiminy/core/stepper/abstract_stepper.h" -#include "jiminy/core/stepper/euler_explicit_stepper.h" -#include "jiminy/core/stepper/runge_kutta_dopri_stepper.h" -#include "jiminy/core/stepper/runge_kutta4_stepper.h" - -#include "jiminy/core/engine/engine_multi_robot.h" - -namespace jiminy -{ - inline constexpr uint32_t INIT_ITERATIONS{4U}; - inline constexpr uint32_t PGS_MAX_ITERATIONS{100U}; - - EngineMultiRobot::EngineMultiRobot() noexcept : - generator_{std::seed_seq{std::random_device{}()}}, - telemetrySender_{std::make_unique()}, - telemetryData_{std::make_shared()}, - telemetryRecorder_{std::make_unique()} - { - // Initialize the configuration options to the default. - setOptions(getDefaultEngineOptions()); - } - - // Cannot be default in the header since some types are incomplete at this point - EngineMultiRobot::~EngineMultiRobot() = default; - - void EngineMultiRobot::addSystem(const std::string & systemName, - std::shared_ptr robot, - std::shared_ptr controller, - const AbortSimulationFunction & callback) - { - // Make sure that no simulation is running - if (isSimulationRunning_) - { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before adding new system."); - } - - if (!robot) - { - THROW_ERROR(std::invalid_argument, "Robot unspecified."); - } - - if (!robot->getIsInitialized()) - { - THROW_ERROR(bad_control_flow, "Robot not initialized."); - } - - if (!controller) - { - THROW_ERROR(std::invalid_argument, "Controller unspecified."); - } - - if (!controller->getIsInitialized()) - { - THROW_ERROR(bad_control_flow, "Controller not initialized."); - } - - auto robot_controller = controller->robot_.lock(); - if (!robot_controller) - { - THROW_ERROR(std::invalid_argument, "Controller's robot expired or unset."); - } - - if (robot != robot_controller) - { - THROW_ERROR(std::invalid_argument, "Controller not initialized for specified robot."); - } - - // TODO: Check that the callback function is working as expected - // FIXME: remove explicit constructor call when moving to C++20 - systems_.emplace_back(System{systemName, robot, controller, callback}); - systemDataVec_.resize(systems_.size()); - } - - void EngineMultiRobot::addSystem(const std::string & systemName, - std::shared_ptr robot, - const AbortSimulationFunction & callback) - { - // Make sure an actual robot is specified - if (!robot) - { - THROW_ERROR(std::invalid_argument, "Robot unspecified."); - } - - // Make sure the robot is properly initialized - if (!robot->getIsInitialized()) - { - THROW_ERROR(std::invalid_argument, "Robot not initialized."); - } - - // When using several robots the robots names are specified - // as a circumfix in the log, for consistency they must all - // have a name - if (systems_.size() && systemName == "") - { - THROW_ERROR(std::invalid_argument, "All systems but the first one must have a name."); - } - - // Check if a system with the same name already exists - auto systemIt = std::find_if(systems_.begin(), - systems_.end(), - [&systemName](const auto & sys) - { return (sys.name == systemName); }); - if (systemIt != systems_.end()) - { - THROW_ERROR(std::invalid_argument, - "System with name '", - systemName, - "'has already been added to the engine."); - } - - // Make sure none of the existing system is referring to the same robot - systemIt = std::find_if(systems_.begin(), - systems_.end(), - [&robot](const auto & sys) { return (sys.robot == robot); }); - if (systemIt != systems_.end()) - { - THROW_ERROR(std::invalid_argument, - "System '", - systemIt->name, - "' already referring to this robot."); - } - - // Create and initialize a controller doing nothing - auto noop = [](double /* t */, - const Eigen::VectorXd & /* q */, - const Eigen::VectorXd & /* v */, - const SensorMeasurementTree & /* sensorMeasurements */, - Eigen::VectorXd & /* out */) - { - // Empty on purpose - }; - auto controller = std::make_shared>(noop, noop); - controller->initialize(robot); - - return addSystem(systemName, robot, controller, callback); - } - - void EngineMultiRobot::removeSystem(const std::string & systemName) - { - // Make sure that no simulation is running - if (isSimulationRunning_) - { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before removing a system."); - } - - /* Remove every coupling forces involving the system. - Note that it is already checking that the system exists. */ - removeCouplingForces(systemName); - - // Get system index - std::ptrdiff_t systemIndex = getSystemIndex(systemName); - - // Update the systems' indices for the remaining coupling forces - for (auto & force : couplingForces_) - { - if (force.systemIndex1 > systemIndex) - { - --force.systemIndex1; - } - if (force.systemIndex2 > systemIndex) - { - --force.systemIndex2; - } - } - - // Remove the system from the list - systems_.erase(systems_.begin() + systemIndex); - systemDataVec_.erase(systemDataVec_.begin() + systemIndex); - } - - void EngineMultiRobot::setController(const std::string & systemName, - std::shared_ptr controller) - { - // Make sure that no simulation is running - if (isSimulationRunning_) - { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before setting a new " - "controller for a system."); - } - - // Make sure that the controller is initialized - if (!controller->getIsInitialized()) - { - THROW_ERROR(bad_control_flow, "Controller not initialized."); - } - - auto robot_controller = controller->robot_.lock(); - if (!robot_controller) - { - THROW_ERROR(bad_control_flow, "Controller's robot expired or unset."); - } - - // Get the system for which to set the controller - System & system = getSystem(systemName); - - if (system.robot != robot_controller) - { - THROW_ERROR(std::invalid_argument, - "Controller not initialized for robot associated with specified system."); - } - - // Set the controller - system.controller = controller; - } - - void EngineMultiRobot::registerCouplingForce(const std::string & systemName1, - const std::string & systemName2, - const std::string & frameName1, - const std::string & frameName2, - const CouplingForceFunction & forceFunc) - { - // Make sure that no simulation is running - if (isSimulationRunning_) - { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before adding coupling forces."); - } - - // Get system and frame indices - const std::ptrdiff_t systemIndex1 = getSystemIndex(systemName1); - const std::ptrdiff_t systemIndex2 = getSystemIndex(systemName2); - const pinocchio::FrameIndex frameIndex1 = - getFrameIndex(systems_[systemIndex1].robot->pinocchioModel_, frameName1); - const pinocchio::FrameIndex frameIndex2 = - getFrameIndex(systems_[systemIndex2].robot->pinocchioModel_, frameName2); - - // Make sure it is not coupling the exact same frame - if (systemIndex1 == systemIndex2 && frameIndex1 == frameIndex2) - { - THROW_ERROR(std::invalid_argument, - "A coupling force must involve two different frames."); - } - - couplingForces_.emplace_back(systemName1, - systemIndex1, - systemName2, - systemIndex2, - frameName1, - frameIndex1, - frameName2, - frameIndex2, - forceFunc); - } - - void EngineMultiRobot::registerViscoelasticCouplingForce(const std::string & systemName1, - const std::string & systemName2, - const std::string & frameName1, - const std::string & frameName2, - const Vector6d & stiffness, - const Vector6d & damping, - double alpha) - { - // Make sure that the input arguments are valid - if ((stiffness.array() < 0.0).any() || (damping.array() < 0.0).any()) - { - THROW_ERROR(std::invalid_argument, - "Stiffness and damping parameters must be positive."); - } - - // Get system and frame indices - System * system1 = &getSystem(systemName1); - System * system2 = &getSystem(systemName2); - pinocchio::FrameIndex frameIndex1 = - getFrameIndex(system1->robot->pinocchioModel_, frameName1); - pinocchio::FrameIndex frameIndex2 = - getFrameIndex(system2->robot->pinocchioModel_, frameName2); - - // Allocate memory - double angle{0.0}; - Eigen::Matrix3d rot12{}, rotJLog12{}, rotJExp12{}, rotRef12{}, omega{}; - Eigen::Vector3d rotLog12{}, pos12{}, posLocal12{}, fLin{}, fAng{}; - - auto forceFunc = [=](double /* t */, - const Eigen::VectorXd & /* q1 */, - const Eigen::VectorXd & /* v1 */, - const Eigen::VectorXd & /* q2 */, - const Eigen::VectorXd & /* v2 */) mutable -> pinocchio::Force - { - /* One must keep track of system pointers and frames indices internally and update - them at reset since the systems may have changed between simulations. Note that - `isSimulationRunning_` is false when called for the first time in `start` method - before locking the robot, so it is the right time to update those proxies. */ - if (!isSimulationRunning_) - { - system1 = &getSystem(systemName1); - system2 = &getSystem(systemName2); - frameIndex1 = getFrameIndex(system1->robot->pinocchioModel_, frameName1); - frameIndex2 = getFrameIndex(system2->robot->pinocchioModel_, frameName2); - } - - // Get the frames positions and velocities in world - const pinocchio::SE3 & oMf1{system1->robot->pinocchioData_.oMf[frameIndex1]}; - const pinocchio::SE3 & oMf2{system2->robot->pinocchioData_.oMf[frameIndex2]}; - const pinocchio::Motion oVf1{getFrameVelocity(system1->robot->pinocchioModel_, - system1->robot->pinocchioData_, - frameIndex1, - pinocchio::LOCAL_WORLD_ALIGNED)}; - const pinocchio::Motion oVf2{getFrameVelocity(system2->robot->pinocchioModel_, - system2->robot->pinocchioData_, - frameIndex2, - pinocchio::LOCAL_WORLD_ALIGNED)}; - - // Compute intermediary quantities - rot12.noalias() = oMf1.rotation().transpose() * oMf2.rotation(); - rotLog12 = pinocchio::log3(rot12, angle); - if (angle > 0.95 * M_PI) - { - THROW_ERROR(std::runtime_error, - "Relative angle between reference frames of viscoelastic " - "coupling must be smaller than 0.95 * pi."); - } - pinocchio::Jlog3(angle, rotLog12, rotJLog12); - fAng = stiffness.tail<3>().array() * rotLog12.array(); - rotLog12 *= alpha; - pinocchio::Jexp3(rotLog12, rotJExp12); - rotRef12.noalias() = oMf1.rotation() * pinocchio::exp3(rotLog12); - pos12 = oMf2.translation() - oMf1.translation(); - posLocal12.noalias() = rotRef12.transpose() * pos12; - fLin = stiffness.head<3>().array() * posLocal12.array(); - omega.noalias() = alpha * rotJExp12 * rotJLog12; - - /* Compute the relative velocity. The application point is the "linear" - interpolation between the frames placement with alpha ratio. */ - const pinocchio::Motion oVf12{oVf2 - oVf1}; - pinocchio::Motion velLocal12{ - rotRef12.transpose() * - (oVf12.linear() + pos12.cross(oVf2.angular() - alpha * oVf12.angular())), - rotRef12.transpose() * oVf12.angular()}; - - // Compute the coupling force acting on frame 2 - pinocchio::Force f{}; - f.linear() = damping.head<3>().array() * velLocal12.linear().array(); - f.angular() = (1.0 - alpha) * f.linear().cross(posLocal12); - f.angular().array() += damping.tail<3>().array() * velLocal12.angular().array(); - f.linear() += fLin; - f.linear() = rotRef12 * f.linear(); - f.angular() = rotRef12 * f.angular(); - f.angular() -= oMf2.rotation() * omega.colwise().cross(posLocal12).transpose() * fLin; - f.angular() += oMf1.rotation() * rotJLog12 * fAng; - - // Deduce the force acting on frame 1 from action-reaction law - f.angular() += pos12.cross(f.linear()); - - return f; - }; - - registerCouplingForce(systemName1, systemName2, frameName1, frameName2, forceFunc); - } - - void EngineMultiRobot::registerViscoelasticCouplingForce(const std::string & systemName, - const std::string & frameName1, - const std::string & frameName2, - const Vector6d & stiffness, - const Vector6d & damping, - double alpha) - { - return registerViscoelasticCouplingForce( - systemName, systemName, frameName1, frameName2, stiffness, damping, alpha); - } - - void EngineMultiRobot::registerViscoelasticDirectionalCouplingForce( - const std::string & systemName1, - const std::string & systemName2, - const std::string & frameName1, - const std::string & frameName2, - double stiffness, - double damping, - double restLength) - { - // Make sure that the input arguments are valid - if (stiffness < 0.0 || damping < 0.0) - { - THROW_ERROR(std::invalid_argument, - "The stiffness and damping parameters must be positive."); - } - - // Get system and frame indices - System * system1 = &getSystem(systemName1); - System * system2 = &getSystem(systemName2); - pinocchio::FrameIndex frameIndex1 = - getFrameIndex(system1->robot->pinocchioModel_, frameName1); - pinocchio::FrameIndex frameIndex2 = - getFrameIndex(system2->robot->pinocchioModel_, frameName2); - - auto forceFunc = [=](double /* t */, - const Eigen::VectorXd & /* q1 */, - const Eigen::VectorXd & /* v1 */, - const Eigen::VectorXd & /* q2 */, - const Eigen::VectorXd & /* v2 */) mutable -> pinocchio::Force - { - /* One must keep track of system pointers and frames indices internally and update - them at reset since the systems may have changed between simulations. Note that - `isSimulationRunning_` is false when called for the first time in `start` method - before locking the robot, so it is the right time to update those proxies. */ - if (!isSimulationRunning_) - { - system1 = &getSystem(systemName1); - system2 = &getSystem(systemName2); - frameIndex1 = getFrameIndex(system1->robot->pinocchioModel_, frameName1); - frameIndex2 = getFrameIndex(system2->robot->pinocchioModel_, frameName2); - } - - // Get the frames positions and velocities in world - const pinocchio::SE3 & oMf1{system1->robot->pinocchioData_.oMf[frameIndex1]}; - const pinocchio::SE3 & oMf2{system2->robot->pinocchioData_.oMf[frameIndex2]}; - const pinocchio::Motion oVf1{getFrameVelocity(system1->robot->pinocchioModel_, - system1->robot->pinocchioData_, - frameIndex1, - pinocchio::LOCAL_WORLD_ALIGNED)}; - const pinocchio::Motion oVf2{getFrameVelocity(system2->robot->pinocchioModel_, - system2->robot->pinocchioData_, - frameIndex2, - pinocchio::LOCAL_WORLD_ALIGNED)}; - - // Compute the linear force coupling them - Eigen::Vector3d dir12{oMf2.translation() - oMf1.translation()}; - const double length{dir12.norm()}; - auto vel12 = oVf2.linear() - oVf1.linear(); - if (length > EPS) - { - dir12 /= length; - auto vel12Proj = vel12.dot(dir12); - return {(stiffness * (length - restLength) + damping * vel12Proj) * dir12, - Eigen::Vector3d::Zero()}; - } - /* The direction between frames is ill-defined, so applying force in the direction - of the linear velocity instead. */ - return {damping * vel12, Eigen::Vector3d::Zero()}; - }; - - registerCouplingForce(systemName1, systemName2, frameName1, frameName2, forceFunc); - } - - void EngineMultiRobot::registerViscoelasticDirectionalCouplingForce( - const std::string & systemName, - const std::string & frameName1, - const std::string & frameName2, - double stiffness, - double damping, - double restLength) - { - return registerViscoelasticDirectionalCouplingForce( - systemName, systemName, frameName1, frameName2, stiffness, damping, restLength); - } - - void EngineMultiRobot::removeCouplingForces(const std::string & systemName1, - const std::string & systemName2) - { - // Make sure that no simulation is running - if (isSimulationRunning_) - { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before removing coupling forces."); - } - - // Make sure that the systems exist - getSystem(systemName1); - getSystem(systemName2); - - // Remove corresponding coupling forces if any - couplingForces_.erase(std::remove_if(couplingForces_.begin(), - couplingForces_.end(), - [&systemName1, &systemName2](const auto & force) { - return (force.systemName1 == systemName1 && - force.systemName2 == systemName2); - }), - couplingForces_.end()); - } - - void EngineMultiRobot::removeCouplingForces(const std::string & systemName) - { - // Make sure that no simulation is running - if (isSimulationRunning_) - { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before removing coupling forces."); - } - - // Make sure that the system exists - getSystem(systemName); - - // Remove corresponding coupling forces if any - couplingForces_.erase(std::remove_if(couplingForces_.begin(), - couplingForces_.end(), - [&systemName](const auto & force) { - return (force.systemName1 == systemName || - force.systemName2 == systemName); - }), - couplingForces_.end()); - } - - void EngineMultiRobot::removeCouplingForces() - { - // Make sure that no simulation is running - if (isSimulationRunning_) - { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before removing coupling forces."); - } - - couplingForces_.clear(); - } - - const CouplingForceVector & EngineMultiRobot::getCouplingForces() const - { - return couplingForces_; - } - - void EngineMultiRobot::removeAllForces() - { - removeCouplingForces(); - removeImpulseForces(); - removeProfileForces(); - } - - void EngineMultiRobot::configureTelemetry() - { - if (systems_.empty()) - { - THROW_ERROR(bad_control_flow, "No system added to the engine."); - } - - if (!isTelemetryConfigured_) - { - // Initialize the engine-specific telemetry sender - telemetrySender_->configure(telemetryData_, ENGINE_TELEMETRY_NAMESPACE); - - auto systemIt = systems_.begin(); - auto systemDataIt = systemDataVec_.begin(); - auto energyIt = energy_.begin(); - for (; systemIt != systems_.end(); ++systemIt, ++systemDataIt, ++energyIt) - { - // Generate the log fieldnames - systemDataIt->logPositionFieldnames = - addCircumfix(systemIt->robot->getLogPositionFieldnames(), - systemIt->name, - {}, - TELEMETRY_FIELDNAME_DELIMITER); - systemDataIt->logVelocityFieldnames = - addCircumfix(systemIt->robot->getLogVelocityFieldnames(), - systemIt->name, - {}, - TELEMETRY_FIELDNAME_DELIMITER); - systemDataIt->logAccelerationFieldnames = - addCircumfix(systemIt->robot->getLogAccelerationFieldnames(), - systemIt->name, - {}, - TELEMETRY_FIELDNAME_DELIMITER); - systemDataIt->logForceExternalFieldnames = - addCircumfix(systemIt->robot->getLogForceExternalFieldnames(), - systemIt->name, - {}, - TELEMETRY_FIELDNAME_DELIMITER); - systemDataIt->logCommandFieldnames = - addCircumfix(systemIt->robot->getLogCommandFieldnames(), - systemIt->name, - {}, - TELEMETRY_FIELDNAME_DELIMITER); - systemDataIt->logMotorEffortFieldnames = - addCircumfix(systemIt->robot->getLogMotorEffortFieldnames(), - systemIt->name, - {}, - TELEMETRY_FIELDNAME_DELIMITER); - systemDataIt->logEnergyFieldname = - addCircumfix("energy", systemIt->name, {}, TELEMETRY_FIELDNAME_DELIMITER); - - // Register variables to the telemetry senders - if (engineOptions_->telemetry.enableConfiguration) - { - telemetrySender_->registerVariable(systemDataIt->logPositionFieldnames, - systemDataIt->state.q); - } - if (engineOptions_->telemetry.enableVelocity) - { - telemetrySender_->registerVariable(systemDataIt->logVelocityFieldnames, - systemDataIt->state.v); - } - if (engineOptions_->telemetry.enableAcceleration) - { - telemetrySender_->registerVariable(systemDataIt->logAccelerationFieldnames, - systemDataIt->state.a); - } - if (engineOptions_->telemetry.enableForceExternal) - { - for (std::size_t i = 1; i < systemDataIt->state.fExternal.size(); ++i) - { - const auto & fext = systemDataIt->state.fExternal[i].toVector(); - for (uint8_t j = 0; j < 6U; ++j) - { - telemetrySender_->registerVariable( - systemDataIt->logForceExternalFieldnames[(i - 1) * 6U + j], - &fext[j]); - } - } - } - if (engineOptions_->telemetry.enableCommand) - { - telemetrySender_->registerVariable(systemDataIt->logCommandFieldnames, - systemDataIt->state.command); - } - if (engineOptions_->telemetry.enableMotorEffort) - { - telemetrySender_->registerVariable(systemDataIt->logMotorEffortFieldnames, - systemDataIt->state.uMotor); - } - if (engineOptions_->telemetry.enableEnergy) - { - telemetrySender_->registerVariable(systemDataIt->logEnergyFieldname, - &(*energyIt)); - } - systemIt->controller->configureTelemetry(telemetryData_, systemIt->name); - systemIt->robot->configureTelemetry(telemetryData_, systemIt->name); - } - } - - isTelemetryConfigured_ = true; - } - - void EngineMultiRobot::updateTelemetry() - { - // Compute the total energy of the system - auto systemIt = systems_.begin(); - auto energyIt = energy_.begin(); - for (; systemIt != systems_.end(); ++systemIt, ++energyIt) - { - *energyIt = systemIt->robot->pinocchioData_.kinetic_energy + - systemIt->robot->pinocchioData_.potential_energy; - } - - // Update system-specific telemetry variables - for (auto & system : systems_) - { - system.controller->updateTelemetry(); - system.robot->updateTelemetry(); - } - - // Update engine-specific telemetry variables - telemetrySender_->updateValues(); - - // Flush the telemetry internal state - telemetryRecorder_->flushSnapshot(stepperState_.t); - } - - void EngineMultiRobot::reset(bool resetRandomNumbers, bool removeAllForce) - { - // Make sure the simulation is properly stopped - if (isSimulationRunning_) - { - stop(); - } - - // Clear log data buffer - logData_.reset(); - - // Reset the dynamic force register if requested - if (removeAllForce) - { - for (auto & systemData : systemDataVec_) - { - systemData.impulseForces.clear(); - systemData.impulseForceBreakpoints.clear(); - systemData.isImpulseForceActiveVec.clear(); - systemData.profileForces.clear(); - } - // FIXME: replaced `std::get` by placeholder `_` when moving to C++26 (P2169R4) - stepperUpdatePeriod_ = - std::get<1>(isGcdIncluded(engineOptions_->stepper.sensorsUpdatePeriod, - engineOptions_->stepper.controllerUpdatePeriod)); - } - - // Reset the random number generators - if (resetRandomNumbers) - { - VectorX seedSeq = engineOptions_->stepper.randomSeedSeq; - generator_.seed(std::seed_seq(seedSeq.data(), seedSeq.data() + seedSeq.size())); - } - - // Reset the internal state of the robot and controller - for (auto & system : systems_) - { - system.robot->reset(generator_); - system.controller->reset(); - } - - // Clear system state buffers, since the robot kinematic may change - for (auto & systemData : systemDataVec_) - { - systemData.state.clear(); - systemData.statePrev.clear(); - } - - isTelemetryConfigured_ = false; - } - - struct ForwardKinematicsAccelerationStep : - public pinocchio::fusion::JointUnaryVisitorBase - { - typedef boost::fusion::vector ArgsType; - - template - static void algo(const pinocchio::JointModelBase & jmodel, - pinocchio::JointDataBase & jdata, - pinocchio::Data & data, - const Eigen::VectorXd & a) - { - pinocchio::JointIndex jointIndex = jmodel.id(); - data.a[jointIndex] = jdata.c() + data.v[jointIndex].cross(jdata.v()); - data.a[jointIndex] += jdata.S() * jmodel.jointVelocitySelector(a); - } - }; - - /// \details This method is optimized to avoid redundant computations. - /// - /// \see See `pinocchio::computeAllTerms` for reference: - /// 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 - void computeExtraTerms(System & system, const SystemData & systemData, ForceVector & fExt) - { - const pinocchio::Model & model = system.robot->pinocchioModel_; - pinocchio::Data & data = system.robot->pinocchioData_; - - // Compute the potential and kinematic energy of the system - pinocchio_overload::computeKineticEnergy( - model, data, systemData.state.q, systemData.state.v, false); - pinocchio::computePotentialEnergy(model, data); - - /* Update manually the subtree (apparent) inertia, since it is only computed by crba, which - is doing more computation than necessary. It will be used here for computing the - centroidal kinematics, and used later for joint bounds dynamics. Note that, by doing all - the computations here instead of 'computeForwardKinematics', we are doing the assumption - that it is varying slowly enough to consider it constant during one integration step. */ - if (!system.robot->hasConstraints()) - { - for (int i = 1; i < model.njoints; ++i) - { - data.Ycrb[i] = model.inertias[i]; - } - for (int jointIndex = model.njoints - 1; jointIndex > 0; --jointIndex) - { - const pinocchio::JointIndex parentJointIndex = model.parents[jointIndex]; - if (parentJointIndex > 0) - { - data.Ycrb[parentJointIndex] += - data.liMi[jointIndex].act(data.Ycrb[jointIndex]); - } - } - } - - /* Neither 'aba' nor 'forwardDynamics' are computing simultaneously the actual joint - accelerations, joint forces and body forces, so it must be done separately: - - 1st step: computing the accelerations based on ForwardKinematic algorithm - - 2nd step: computing the forces based on RNEA algorithm */ - - /* Compute the true joint acceleration and the one due to the lone gravity field, then - the spatial momenta and the total internal and external forces acting on each body. */ - data.h[0].setZero(); - fExt[0].setZero(); - data.f[0].setZero(); - data.a[0].setZero(); - data.a_gf[0] = -model.gravity; - for (int jointIndex = 1; jointIndex < model.njoints; ++jointIndex) - { - ForwardKinematicsAccelerationStep::run( - model.joints[jointIndex], - data.joints[jointIndex], - typename ForwardKinematicsAccelerationStep::ArgsType(data, systemData.state.a)); - - const pinocchio::JointIndex parentJointIndex = model.parents[jointIndex]; - data.a_gf[jointIndex] = data.a[jointIndex]; - data.a[jointIndex] += data.liMi[jointIndex].actInv(data.a[parentJointIndex]); - data.a_gf[jointIndex] += data.liMi[jointIndex].actInv(data.a_gf[parentJointIndex]); - - model.inertias[jointIndex].__mult__(data.v[jointIndex], data.h[jointIndex]); - - model.inertias[jointIndex].__mult__(data.a[jointIndex], fExt[jointIndex]); - data.f[jointIndex] = data.v[jointIndex].cross(data.h[jointIndex]); - fExt[jointIndex] += data.f[jointIndex]; - data.f[jointIndex] += model.inertias[jointIndex] * data.a_gf[jointIndex]; - data.f[jointIndex] -= systemData.state.fExternal[jointIndex]; - } - for (int jointIndex = model.njoints - 1; jointIndex > 0; --jointIndex) - { - const pinocchio::JointIndex parentJointIndex = model.parents[jointIndex]; - fExt[parentJointIndex] += data.liMi[jointIndex].act(fExt[jointIndex]); - data.h[parentJointIndex] += data.liMi[jointIndex].act(data.h[jointIndex]); - if (parentJointIndex > 0) - { - data.f[parentJointIndex] += data.liMi[jointIndex].act(data.f[jointIndex]); - } - } - - // Compute the position and velocity of the center of mass of each subtree - for (int jointIndex = 0; jointIndex < model.njoints; ++jointIndex) - { - if (jointIndex > 0) - { - data.com[jointIndex] = data.Ycrb[jointIndex].lever(); - } - data.vcom[jointIndex].noalias() = data.h[jointIndex].linear() / data.mass[jointIndex]; - } - data.com[0] = data.liMi[1].act(data.com[1]); - - // Compute centroidal dynamics and its derivative - data.hg = data.h[0]; - data.hg.angular() += data.hg.linear().cross(data.com[0]); - data.dhg = fExt[0]; - data.dhg.angular() += data.dhg.linear().cross(data.com[0]); - } - - void computeAllExtraTerms(std::vector & systems, - const vector_aligned_t & systemDataVec, - vector_aligned_t & f) - { - auto systemIt = systems.begin(); - auto systemDataIt = systemDataVec.begin(); - auto fIt = f.begin(); - for (; systemIt != systems.end(); ++systemIt, ++systemDataIt, ++fIt) - { - computeExtraTerms(*systemIt, *systemDataIt, *fIt); - } - } - - void syncAccelerationsAndForces( - const System & system, ForceVector & contactForces, ForceVector & f, MotionVector & a) - { - for (std::size_t i = 0; i < system.robot->getContactFrameNames().size(); ++i) - { - contactForces[i] = system.robot->contactForces_[i]; - } - - for (int i = 0; i < system.robot->pinocchioModel_.njoints; ++i) - { - f[i] = system.robot->pinocchioData_.f[i]; - a[i] = system.robot->pinocchioData_.a[i]; - } - } - - void syncAllAccelerationsAndForces(const std::vector & systems, - vector_aligned_t & contactForces, - vector_aligned_t & f, - vector_aligned_t & a) - { - std::vector::const_iterator systemIt = systems.begin(); - auto contactForceIt = contactForces.begin(); - auto fPrevIt = f.begin(); - auto aPrevIt = a.begin(); - for (; systemIt != systems.end(); ++systemIt, ++contactForceIt, ++fPrevIt, ++aPrevIt) - { - syncAccelerationsAndForces(*systemIt, *contactForceIt, *fPrevIt, *aPrevIt); - } - } - - void EngineMultiRobot::start( - const std::map & qInit, - const std::map & vInit, - const std::optional> & aInit) - { - // Make sure that no simulation is running - if (isSimulationRunning_) - { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before starting again."); - } - - if (systems_.empty()) - { - THROW_ERROR(bad_control_flow, - "No system to simulate. Please add one before starting a simulation."); - } - - const std::size_t nSystems = systems_.size(); - if (qInit.size() != nSystems || vInit.size() != nSystems) - { - THROW_ERROR(std::invalid_argument, - "The number of initial configurations and velocities must " - "match the number of systems."); - } - - // Check the dimension of the initial state associated with every system and order them - std::vector qSplit; - std::vector vSplit; - qSplit.reserve(nSystems); - vSplit.reserve(nSystems); - for (const auto & system : systems_) - { - auto qInitIt = qInit.find(system.name); - auto vInitIt = vInit.find(system.name); - if (qInitIt == qInit.end() || vInitIt == vInit.end()) - { - THROW_ERROR(std::invalid_argument, - "System '", - system.name, - "'does not have an initial configuration or velocity."); - } - - const Eigen::VectorXd & q = qInitIt->second; - const Eigen::VectorXd & v = vInitIt->second; - if (q.rows() != system.robot->nq() || v.rows() != system.robot->nv()) - { - THROW_ERROR(std::invalid_argument, - "The dimension of the initial configuration or velocity is " - "inconsistent with model size for system '", - system.name, - "'."); - } - - // Make sure the initial state is normalized - const bool isValid = isPositionValid( - system.robot->pinocchioModel_, q, std::numeric_limits::epsilon()); - if (!isValid) - { - THROW_ERROR(std::invalid_argument, - "The initial configuration is not consistent with the types of joints " - "of the model for system '", - system.name, - "'."); - } - - /* Check that the initial configuration is not out-of-bounds if appropriate. - Note that EPS allows to be very slightly out-of-bounds, which may occurs because of - rounding errors. */ - if ((system.robot->modelOptions_->joints.enablePositionLimit && - (contactModel_ == ContactModelType::CONSTRAINT) && - ((EPS < q.array() - system.robot->getPositionLimitMax().array()).any() || - (EPS < system.robot->getPositionLimitMin().array() - q.array()).any()))) - { - THROW_ERROR(std::invalid_argument, - "The initial configuration is out-of-bounds for system '", - system.name, - "'."); - } - - // Check that the initial velocity is not out-of-bounds - if ((system.robot->modelOptions_->joints.enableVelocityLimit && - (system.robot->getVelocityLimit().array() < v.array().abs()).any())) - { - THROW_ERROR(std::invalid_argument, - "The initial velocity is out-of-bounds for system '", - system.name, - "'."); - } - - /* Make sure the configuration is normalized (as double), since normalization is - checked using float accuracy rather than double to circumvent float/double casting - than may occurs because of Python bindings. */ - Eigen::VectorXd qNormalized = q; - pinocchio::normalize(system.robot->pinocchioModel_, qNormalized); - - qSplit.emplace_back(qNormalized); - vSplit.emplace_back(v); - } - - std::vector aSplit; - aSplit.reserve(nSystems); - if (aInit) - { - // Check the dimension of the initial acceleration associated with every system - if (aInit->size() != nSystems) - { - THROW_ERROR(std::invalid_argument, - "If specified, the number of initial accelerations " - "must match the number of systems."); - } - - for (const auto & system : systems_) - { - auto aInitIt = aInit->find(system.name); - if (aInitIt == aInit->end()) - { - THROW_ERROR(std::invalid_argument, - "System '", - system.name, - "'does not have an initial acceleration."); - } - - const Eigen::VectorXd & a = aInitIt->second; - if (a.rows() != system.robot->nv()) - { - THROW_ERROR(std::invalid_argument, - "The dimension of the initial acceleration is inconsistent with " - "model size for system '", - system.name, - "'."); - } - - aSplit.emplace_back(a); - } - } - else - { - // Zero acceleration by default - std::transform(vSplit.begin(), - vSplit.end(), - std::back_inserter(aSplit), - [](const auto & v) -> Eigen::VectorXd - { return Eigen::VectorXd::Zero(v.size()); }); - } - - for (auto & system : systems_) - { - for (const auto & sensorGroupItem : system.robot->getSensors()) - { - for (const auto & sensor : sensorGroupItem.second) - { - if (!sensor->getIsInitialized()) - { - THROW_ERROR(bad_control_flow, - "At least a sensor of a robot is not initialized."); - } - } - } - - for (const auto & motor : system.robot->getMotors()) - { - if (!motor->getIsInitialized()) - { - THROW_ERROR(bad_control_flow, - "At least a motor of a robot is not initialized."); - } - } - } - - /* Call reset if the internal state of the engine is not clean. Not doing it systematically - gives the opportunity to the user to customize the system by resetting first the engine - manually and then to alter the system before starting a simulation, e.g. to change the - inertia of a specific body. */ - if (isTelemetryConfigured_) - { - reset(false, false); - } - - // Reset the internal state of the robot and controller - auto systemIt = systems_.begin(); - auto systemDataIt = systemDataVec_.begin(); - for (; systemIt != systems_.end(); ++systemIt, ++systemDataIt) - { - // Propagate the user-defined gravity at robot level - systemIt->robot->pinocchioModelOrig_.gravity = engineOptions_->world.gravity; - systemIt->robot->pinocchioModel_.gravity = engineOptions_->world.gravity; - - /* Reinitialize the system state buffers, since the robot kinematic may have changed. - For example, it may happens if one activates or deactivates the flexibility between - two successive simulations. */ - systemDataIt->state.initialize(*(systemIt->robot)); - systemDataIt->statePrev.initialize(*(systemIt->robot)); - systemDataIt->successiveSolveFailed = 0U; - } - - // Initialize the ode solver - auto systemOde = [this](double t, - const std::vector & q, - const std::vector & v, - std::vector & a) -> void - { - this->computeSystemsDynamics(t, q, v, a); - }; - std::vector robots; - robots.reserve(nSystems); - std::transform(systems_.begin(), - systems_.end(), - std::back_inserter(robots), - [](const auto & sys) -> const Robot * { return sys.robot.get(); }); - if (engineOptions_->stepper.odeSolver == "runge_kutta_dopri5") - { - stepper_ = std::unique_ptr( - new RungeKuttaDOPRIStepper(systemOde, - robots, - engineOptions_->stepper.tolAbs, - engineOptions_->stepper.tolRel)); - } - else if (engineOptions_->stepper.odeSolver == "runge_kutta_4") - { - stepper_ = std::unique_ptr(new RungeKutta4Stepper(systemOde, robots)); - } - else if (engineOptions_->stepper.odeSolver == "euler_explicit") - { - stepper_ = - std::unique_ptr(new EulerExplicitStepper(systemOde, robots)); - } - - // Initialize the stepper state - const double t = 0.0; - stepperState_.reset(SIMULATION_MIN_TIMESTEP, qSplit, vSplit, aSplit); - - // Initialize previous joints forces and accelerations - contactForcesPrev_.clear(); - contactForcesPrev_.reserve(nSystems); - fPrev_.clear(); - fPrev_.reserve(nSystems); - aPrev_.clear(); - aPrev_.reserve(nSystems); - for (const auto & system : systems_) - { - contactForcesPrev_.push_back(system.robot->contactForces_); - fPrev_.push_back(system.robot->pinocchioData_.f); - aPrev_.push_back(system.robot->pinocchioData_.a); - } - energy_.resize(nSystems, 0.0); - - // Synchronize the individual system states with the global stepper state - syncSystemsStateWithStepper(); - - // Update the frame indices associated with the coupling forces - for (auto & force : couplingForces_) - { - force.frameIndex1 = getFrameIndex(systems_[force.systemIndex1].robot->pinocchioModel_, - force.frameName1); - force.frameIndex2 = getFrameIndex(systems_[force.systemIndex2].robot->pinocchioModel_, - force.frameName2); - } - - systemIt = systems_.begin(); - systemDataIt = systemDataVec_.begin(); - for (; systemIt != systems_.end(); ++systemIt, ++systemDataIt) - { - // Update the frame indices associated with the impulse forces and force profiles - for (auto & force : systemDataIt->profileForces) - { - force.frameIndex = - getFrameIndex(systemIt->robot->pinocchioModel_, force.frameName); - } - for (auto & force : systemDataIt->impulseForces) - { - force.frameIndex = - getFrameIndex(systemIt->robot->pinocchioModel_, force.frameName); - } - - // Initialize the impulse force breakpoint point iterator - systemDataIt->impulseForceBreakpointNextIt = - systemDataIt->impulseForceBreakpoints.begin(); - - // Reset the active set of impulse forces - std::fill(systemDataIt->isImpulseForceActiveVec.begin(), - systemDataIt->isImpulseForceActiveVec.end(), - false); - - // Activate every force impulse starting at t=0 - auto isImpulseForceActiveIt = systemDataIt->isImpulseForceActiveVec.begin(); - auto impulseForceIt = systemDataIt->impulseForces.begin(); - for (; impulseForceIt != systemDataIt->impulseForces.end(); - ++isImpulseForceActiveIt, ++impulseForceIt) - { - if (impulseForceIt->t < STEPPER_MIN_TIMESTEP) - { - *isImpulseForceActiveIt = true; - } - } - - // Compute the forward kinematics for each system - const Eigen::VectorXd & q = systemDataIt->state.q; - const Eigen::VectorXd & v = systemDataIt->state.v; - const Eigen::VectorXd & a = systemDataIt->state.a; - computeForwardKinematics(*systemIt, q, v, a); - - /* Backup constraint register for fast lookup. - Internal constraints cannot be added/removed at this point. */ - systemDataIt->constraints = systemIt->robot->getConstraints(); - - // Initialize contacts forces in local frame - const std::vector & contactFrameIndices = - systemIt->robot->getContactFrameIndices(); - systemDataIt->contactFrameForces = - ForceVector(contactFrameIndices.size(), pinocchio::Force::Zero()); - const std::vector> & collisionPairIndices = - systemIt->robot->getCollisionPairIndices(); - systemDataIt->collisionBodiesForces.clear(); - systemDataIt->collisionBodiesForces.reserve(collisionPairIndices.size()); - for (const auto & bodyCollisionPairIndices : collisionPairIndices) - { - systemDataIt->collisionBodiesForces.emplace_back(bodyCollisionPairIndices.size(), - pinocchio::Force::Zero()); - } - - /* Initialize some addition buffers used by impulse contact solver. - It must be initialized to zero because 'getJointJacobian' will only update non-zero - coefficients for efficiency. */ - systemDataIt->jointJacobians.resize( - systemIt->robot->pinocchioModel_.njoints, - Matrix6Xd::Zero(6, systemIt->robot->pinocchioModel_.nv)); - - // Reset the constraints - systemIt->robot->resetConstraints(q, v); - - /* Set Baumgarte stabilization natural frequency for contact constraints - Enable all contact constraints by default, it will be disable automatically if not - in contact. It is useful to start in post-hysteresis state to avoid discontinuities - at init. */ - systemDataIt->constraints.foreach( - [&contactModel = contactModel_, - &enablePositionLimit = systemIt->robot->modelOptions_->joints.enablePositionLimit, - &freq = engineOptions_->contacts.stabilizationFreq]( - const std::shared_ptr & constraint, - ConstraintNodeType node) - { - // Set baumgarte freq for all contact constraints - if (node != ConstraintNodeType::USER) - { - constraint->setBaumgarteFreq(freq); // It cannot fail - } - - // Enable constraints by default - if (contactModel == ContactModelType::CONSTRAINT) - { - switch (node) - { - case ConstraintNodeType::BOUNDS_JOINTS: - if (!enablePositionLimit) - { - return; - } - { - auto & jointConstraint = - static_cast(*constraint.get()); - jointConstraint.setRotationDir(false); - } - [[fallthrough]]; - case ConstraintNodeType::CONTACT_FRAMES: - case ConstraintNodeType::COLLISION_BODIES: - constraint->enable(); - break; - case ConstraintNodeType::USER: - default: - break; - } - } - }); - - if (contactModel_ == ContactModelType::SPRING_DAMPER) - { - // Make sure that the contact forces are bounded for spring-damper model. - // TODO: Rather use something like `10 * m * g` instead of a fix threshold. - double forceMax = 0.0; - for (std::size_t i = 0; i < contactFrameIndices.size(); ++i) - { - auto & constraint = systemDataIt->constraints.contactFrames[i].second; - pinocchio::Force & fextLocal = systemDataIt->contactFrameForces[i]; - computeContactDynamicsAtFrame( - *systemIt, contactFrameIndices[i], constraint, fextLocal); - forceMax = std::max(forceMax, fextLocal.linear().norm()); - } - - for (std::size_t i = 0; i < collisionPairIndices.size(); ++i) - { - for (std::size_t j = 0; j < collisionPairIndices[i].size(); ++j) - { - const pinocchio::PairIndex & collisionPairIndex = - collisionPairIndices[i][j]; - auto & constraint = systemDataIt->constraints.collisionBodies[i][j].second; - pinocchio::Force & fextLocal = systemDataIt->collisionBodiesForces[i][j]; - computeContactDynamicsAtBody( - *systemIt, collisionPairIndex, constraint, fextLocal); - forceMax = std::max(forceMax, fextLocal.linear().norm()); - } - } - - if (forceMax > 1e5) - { - THROW_ERROR( - std::invalid_argument, - "The initial force exceeds 1e5 for at least one contact point, which is " - "forbidden for the sake of numerical stability. Please update the initial " - "state."); - } - } - } - - // Lock the robots. At this point, it is no longer possible to change them anymore. - systemIt = systems_.begin(); - systemDataIt = systemDataVec_.begin(); - for (; systemIt != systems_.end(); ++systemIt, ++systemDataIt) - { - systemDataIt->robotLock = systemIt->robot->getLock(); - } - - // Instantiate the desired LCP solver - systemIt = systems_.begin(); - systemDataIt = systemDataVec_.begin(); - for (; systemIt != systems_.end(); ++systemIt, ++systemDataIt) - { - const std::string & constraintSolverType = engineOptions_->constraints.solver; - switch (CONSTRAINT_SOLVERS_MAP.at(constraintSolverType)) - { - case ConstraintSolverType::PGS: - systemDataIt->constraintSolver = - std::make_unique(&systemIt->robot->pinocchioModel_, - &systemIt->robot->pinocchioData_, - &systemDataIt->constraints, - engineOptions_->contacts.friction, - engineOptions_->contacts.torsion, - engineOptions_->stepper.tolAbs, - engineOptions_->stepper.tolRel, - PGS_MAX_ITERATIONS); - break; - case ConstraintSolverType::UNSUPPORTED: - default: - break; - } - } - - /* Compute the efforts, internal and external forces applied on every systems excluding - user-specified internal dynamics if any. */ - computeAllTerms(t, qSplit, vSplit); - - // Backup all external forces and internal efforts excluding constraint forces - vector_aligned_t fextNoConst; - std::vector uInternalConst; - fextNoConst.reserve(nSystems); - uInternalConst.reserve(nSystems); - for (const auto & systemData : systemDataVec_) - { - fextNoConst.push_back(systemData.state.fExternal); - uInternalConst.push_back(systemData.state.uInternal); - } - - /* Solve algebraic coupling between accelerations, sensors and controllers, by - iterating several times until it (hopefully) converges. */ - bool isFirstIter = true; - for (uint32_t i = 0; i < INIT_ITERATIONS; ++i) - { - systemIt = systems_.begin(); - systemDataIt = systemDataVec_.begin(); - auto fextNoConstIt = fextNoConst.begin(); - auto uInternalConstIt = uInternalConst.begin(); - for (; systemIt != systems_.end(); - ++systemIt, ++systemDataIt, ++fextNoConstIt, ++uInternalConstIt) - { - // Get some system state proxies - const Eigen::VectorXd & q = systemDataIt->state.q; - const Eigen::VectorXd & v = systemDataIt->state.v; - Eigen::VectorXd & a = systemDataIt->state.a; - Eigen::VectorXd & u = systemDataIt->state.u; - Eigen::VectorXd & command = systemDataIt->state.command; - Eigen::VectorXd & uMotor = systemDataIt->state.uMotor; - Eigen::VectorXd & uInternal = systemDataIt->state.uInternal; - Eigen::VectorXd & uCustom = systemDataIt->state.uCustom; - ForceVector & fext = systemDataIt->state.fExternal; - - // Reset the external forces and internal efforts - fext = *fextNoConstIt; - uInternal = *uInternalConstIt; - - // Compute dynamics - a = computeAcceleration( - *systemIt, *systemDataIt, q, v, u, fext, !isFirstIter, isFirstIter); - - // Make sure there is no nan at this point - if ((a.array() != a.array()).any()) - { - THROW_ERROR(std::runtime_error, - "Impossible to compute the acceleration. Probably a " - "subtree has zero inertia along an articulated axis."); - } - - // Compute all external terms including joints accelerations and forces - computeAllExtraTerms(systems_, systemDataVec_, fPrev_); - - // Compute the sensor data with the updated effort and acceleration - systemIt->robot->computeSensorMeasurements(t, q, v, a, uMotor, fext); - - // Compute the actual motor effort - computeCommand(*systemIt, t, q, v, command); - - // Compute the actual motor effort - systemIt->robot->computeMotorEfforts(t, q, v, a, command); - uMotor = systemIt->robot->getMotorEfforts(); - - // Compute the internal dynamics - uCustom.setZero(); - systemIt->controller->internalDynamics(t, q, v, uCustom); - - // Compute the total effort vector - u = uInternal + uCustom; - for (const auto & motor : systemIt->robot->getMotors()) - { - const std::size_t motorIndex = motor->getIndex(); - const Eigen::Index motorVelocityIndex = motor->getJointVelocityIndex(); - u[motorVelocityIndex] += uMotor[motorIndex]; - } - } - isFirstIter = false; - } - - // Update sensor data one last time to take into account the actual motor effort - systemIt = systems_.begin(); - systemDataIt = systemDataVec_.begin(); - for (; systemIt != systems_.end(); ++systemIt, ++systemDataIt) - { - const Eigen::VectorXd & q = systemDataIt->state.q; - const Eigen::VectorXd & v = systemDataIt->state.v; - const Eigen::VectorXd & a = systemDataIt->state.a; - const Eigen::VectorXd & uMotor = systemDataIt->state.uMotor; - const ForceVector & fext = systemDataIt->state.fExternal; - systemIt->robot->computeSensorMeasurements(t, q, v, a, uMotor, fext); - } - - // Backend the updated joint accelerations and forces - syncAllAccelerationsAndForces(systems_, contactForcesPrev_, fPrev_, aPrev_); - - // Synchronize the global stepper state with the individual system states - syncStepperStateWithSystems(); - - // Initialize the last system states - for (auto & systemData : systemDataVec_) - { - systemData.statePrev = systemData.state; - } - - // Lock the telemetry. At this point it is not possible to register new variables. - configureTelemetry(); - - // Log systems data - for (const auto & system : systems_) - { - // Backup URDF file - const std::string telemetryUrdfFile = - addCircumfix("urdf_file", system.name, {}, TELEMETRY_FIELDNAME_DELIMITER); - const std::string & urdfFileString = system.robot->getUrdfAsString(); - telemetrySender_->registerConstant(telemetryUrdfFile, urdfFileString); - - // Backup 'has_freeflyer' option - const std::string telemetrHasFreeflyer = - addCircumfix("has_freeflyer", system.name, {}, TELEMETRY_FIELDNAME_DELIMITER); - telemetrySender_->registerConstant(telemetrHasFreeflyer, - toString(system.robot->getHasFreeflyer())); - - // Backup mesh package lookup directories - const std::string telemetryMeshPackageDirs = - addCircumfix("mesh_package_dirs", system.name, {}, TELEMETRY_FIELDNAME_DELIMITER); - std::string meshPackageDirsString; - std::stringstream meshPackageDirsStream; - const std::vector & meshPackageDirs = system.robot->getMeshPackageDirs(); - copy(meshPackageDirs.begin(), - meshPackageDirs.end(), - std::ostream_iterator(meshPackageDirsStream, ";")); - if (meshPackageDirsStream.peek() != - decltype(meshPackageDirsStream)::traits_type::eof()) - { - meshPackageDirsString = meshPackageDirsStream.str(); - meshPackageDirsString.pop_back(); - } - telemetrySender_->registerConstant(telemetryMeshPackageDirs, meshPackageDirsString); - - // Backup the true and theoretical Pinocchio::Model - std::string key = - addCircumfix("pinocchio_model", system.name, {}, TELEMETRY_FIELDNAME_DELIMITER); - std::string value = saveToBinary(system.robot->pinocchioModel_); - telemetrySender_->registerConstant(key, value); - - /* Backup the Pinocchio GeometryModel for collisions and visuals. - It may fail because of missing serialization methods for convex, or because it - cannot fit into memory (return code). Persistent mode is automatically enforced - if no URDF is associated with the robot.*/ - if (engineOptions_->telemetry.isPersistent || urdfFileString.empty()) - { - try - { - key = addCircumfix( - "collision_model", system.name, {}, TELEMETRY_FIELDNAME_DELIMITER); - value = saveToBinary(system.robot->collisionModel_); - telemetrySender_->registerConstant(key, value); - - key = addCircumfix( - "visual_model", system.name, {}, TELEMETRY_FIELDNAME_DELIMITER); - value = saveToBinary(system.robot->visualModel_); - telemetrySender_->registerConstant(key, value); - } - catch (const std::exception & e) - { - std::string msg{"Failed to log the collision and/or visual model."}; - if (urdfFileString.empty()) - { - msg += " It will be impossible to replay log files because no URDF " - "file is available as fallback."; - } - msg += "\nRaised from exception: "; - PRINT_WARNING(msg, e.what()); - } - } - } - - // Log all options - GenericConfig allOptions; - for (const auto & system : systems_) - { - const std::string telemetryRobotOptions = - addCircumfix("system", system.name, {}, TELEMETRY_FIELDNAME_DELIMITER); - GenericConfig systemOptions; - systemOptions["robot"] = system.robot->getOptions(); - systemOptions["controller"] = system.controller->getOptions(); - allOptions[telemetryRobotOptions] = systemOptions; - } - allOptions["engine"] = engineOptionsGeneric_; - Json::Value allOptionsJson = convertToJson(allOptions); - Json::StreamWriterBuilder jsonWriter; - jsonWriter["indentation"] = ""; - const std::string allOptionsString = Json::writeString(jsonWriter, allOptionsJson); - telemetrySender_->registerConstant("options", allOptionsString); - - // Write the header: this locks the registration of new variables - telemetryRecorder_->initialize(telemetryData_.get(), getTelemetryTimeUnit()); - - // At this point, consider that the simulation is running - isSimulationRunning_ = true; - } - - void EngineMultiRobot::simulate( - double tEnd, - const std::map & qInit, - const std::map & vInit, - const std::optional> & aInit) - { - if (systems_.empty()) - { - THROW_ERROR(bad_control_flow, - "No system to simulate. Please add one before starting simulation."); - } - - if (tEnd < 5e-3) - { - THROW_ERROR(std::invalid_argument, "Simulation duration cannot be shorter than 5ms."); - } - - // Reset the robot, controller, and engine - reset(true, false); - - // Start the simulation - start(qInit, vInit, aInit); - - // Now that telemetry has been initialized, check simulation duration - if (tEnd > telemetryRecorder_->getLogDurationMax()) - { - THROW_ERROR(std::runtime_error, - "Time overflow: with the current precision the maximum value that can be " - "logged is ", - telemetryRecorder_->getLogDurationMax(), - "s. Decrease logger precision to simulate for longer than that."); - } - - // Integration loop based on boost::numeric::odeint::detail::integrate_times - while (true) - { - // Stop the simulation if the end time has been reached - if (tEnd - stepperState_.t < SIMULATION_MIN_TIMESTEP) - { - if (engineOptions_->stepper.verbose) - { - std::cout << "Simulation done: desired final time reached." << std::endl; - } - break; - } - - // Stop the simulation if any of the callbacks return false - bool isCallbackFalse = false; - auto systemIt = systems_.begin(); - auto systemDataIt = systemDataVec_.begin(); - for (; systemIt != systems_.end(); ++systemIt, ++systemDataIt) - { - if (!systemIt->callback( - stepperState_.t, systemDataIt->state.q, systemDataIt->state.v)) - { - isCallbackFalse = true; - break; - } - } - if (isCallbackFalse) - { - if (engineOptions_->stepper.verbose) - { - std::cout << "Simulation done: callback returned false." << std::endl; - } - break; - } - - // Stop the simulation if the max number of integration steps is reached - if (0U < engineOptions_->stepper.iterMax && - engineOptions_->stepper.iterMax <= stepperState_.iter) - { - if (engineOptions_->stepper.verbose) - { - std::cout << "Simulation done: maximum number of integration steps exceeded." - << std::endl; - } - break; - } - - // Perform a single integration step up to `tEnd`, stopping at `stepperUpdatePeriod_` - double stepSize; - if (std::isfinite(stepperUpdatePeriod_)) - { - stepSize = std::min(stepperUpdatePeriod_, tEnd - stepperState_.t); - } - else - { - stepSize = std::min(engineOptions_->stepper.dtMax, tEnd - stepperState_.t); - } - step(stepSize); // Automatic dt adjustment - } - - // Stop the simulation. The lock on the telemetry and the robots are released. - stop(); - } - - void EngineMultiRobot::step(double stepSize) - { - // Check if the simulation has started - if (!isSimulationRunning_) - { - THROW_ERROR(bad_control_flow, - "No simulation running. Please start one before using step method."); - } - - // Clear log data buffer - logData_.reset(); - - // Check if there is something wrong with the integration - auto qIt = stepperState_.qSplit.begin(); - auto vIt = stepperState_.vSplit.begin(); - auto aIt = stepperState_.aSplit.begin(); - for (; qIt != stepperState_.qSplit.end(); ++qIt, ++vIt, ++aIt) - { - if (qIt->hasNaN() || vIt->hasNaN() || aIt->hasNaN()) - { - THROW_ERROR(std::runtime_error, - "Low-level ode solver failed. Consider increasing stepper accuracy."); - } - } - - // Check if the desired step size is suitable - if (stepSize > EPS && stepSize < SIMULATION_MIN_TIMESTEP) - { - THROW_ERROR(std::invalid_argument, "Step size out of bounds."); - } - - /* Set end time: The default step size is equal to the controller update period if - discrete-time, otherwise it uses the sensor update period if discrete-time, otherwise - it uses the user-defined parameter dtMax. */ - if (stepSize < EPS) - { - const double controllerUpdatePeriod = engineOptions_->stepper.controllerUpdatePeriod; - if (controllerUpdatePeriod > EPS) - { - stepSize = controllerUpdatePeriod; - } - else - { - const double sensorsUpdatePeriod = engineOptions_->stepper.sensorsUpdatePeriod; - if (sensorsUpdatePeriod > EPS) - { - stepSize = sensorsUpdatePeriod; - } - else - { - stepSize = engineOptions_->stepper.dtMax; - } - } - } - - /* Check that end time is not too large for the current logging precision, otherwise abort - integration. */ - if (stepperState_.t + stepSize > telemetryRecorder_->getLogDurationMax()) - { - THROW_ERROR(std::runtime_error, - "Time overflow: with the current precision the maximum value that can be " - "logged is ", - telemetryRecorder_->getLogDurationMax(), - "s. Decrease logger precision to simulate for longer than that."); - } - - /* Avoid compounding of error using Kahan algorithm. It consists in keeping track of the - cumulative rounding error to add it back to the sum when it gets larger than the - numerical precision, thus avoiding it to grows unbounded. */ - const double stepSizeCorrected = stepSize - stepperState_.tError; - const double tEnd = stepperState_.t + stepSizeCorrected; - stepperState_.tError = (tEnd - stepperState_.t) - stepSizeCorrected; - - // Get references to some internal stepper buffers - double & t = stepperState_.t; - double & dt = stepperState_.dt; - double & dtLargest = stepperState_.dtLargest; - std::vector & qSplit = stepperState_.qSplit; - std::vector & vSplit = stepperState_.vSplit; - std::vector & aSplit = stepperState_.aSplit; - - // Monitor iteration failure - uint32_t successiveIterFailed = 0; - std::vector successiveSolveFailedAll(systems_.size(), 0U); - bool isNan = false; - - /* Flag monitoring if the current time step depends of a breakpoint or the integration - tolerance. It will be used by the restoration mechanism, if dt gets very small to reach - a breakpoint, in order to avoid having to perform several steps to stabilize again the - estimation of the optimal time step. */ - bool isBreakpointReached = false; - - /* Flag monitoring if the dynamics has changed because of impulse forces or the command - (only in the case of discrete control). - - `tryStep(rhs, x, dxdt, t, dt)` method of error controlled boost steppers leverage the - FSAL (first same as last) principle. It is implemented by considering at the value of - (x, dxdt) in argument have been initialized by the user with the system dynamics at - current time t. Thus, if the system dynamics is discontinuous, one has to manually - integrate up to t-, then update dxdt to take into the acceleration at t+. - - Note that ONLY the acceleration part of dxdt must be updated since the velocity is not - supposed to have changed. On top of that, tPrev is invalid at this point because it has - been updated just after the last successful step. - - TODO: Maybe dt should be reschedule because the dynamics has changed and thereby the - previously estimated dt is very meaningful anymore. */ - bool hasDynamicsChanged = false; - - // Start the timer used for timeout handling - timer_.tic(); - - // Perform the integration. Do not simulate extremely small time steps. - while (tEnd - t >= STEPPER_MIN_TIMESTEP) - { - // Initialize next breakpoint time to the one recommended by the stepper - double tNext = t; - - // Update the active set and get the next breakpoint of impulse forces - double tImpulseForceNext = INF; - for (auto & systemData : systemDataVec_) - { - /* Update the active set: activate an impulse force as soon as the current time - gets close enough of the application time, and deactivate it once the following - the same reasoning. - - Note that breakpoints at the start/end of every impulse forces are already - enforced, so that the forces cannot get activated/deactivate too late. */ - auto isImpulseForceActiveIt = systemData.isImpulseForceActiveVec.begin(); - auto impulseForceIt = systemData.impulseForces.begin(); - for (; impulseForceIt != systemData.impulseForces.end(); - ++isImpulseForceActiveIt, ++impulseForceIt) - { - double tImpulseForce = impulseForceIt->t; - double dtImpulseForce = impulseForceIt->dt; - - if (t > tImpulseForce - STEPPER_MIN_TIMESTEP) - { - *isImpulseForceActiveIt = true; - hasDynamicsChanged = true; - } - if (t >= tImpulseForce + dtImpulseForce - STEPPER_MIN_TIMESTEP) - { - *isImpulseForceActiveIt = false; - hasDynamicsChanged = true; - } - } - - // Update the breakpoint time iterator if necessary - auto & tBreakpointNextIt = systemData.impulseForceBreakpointNextIt; - if (tBreakpointNextIt != systemData.impulseForceBreakpoints.end()) - { - if (t >= *tBreakpointNextIt - STEPPER_MIN_TIMESTEP) - { - // The current breakpoint is behind in time. Switching to the next one. - ++tBreakpointNextIt; - } - } - - // Get the next breakpoint time if any - if (tBreakpointNextIt != systemData.impulseForceBreakpoints.end()) - { - tImpulseForceNext = std::min(tImpulseForceNext, *tBreakpointNextIt); - } - } - - // Update the external force profiles if necessary (only for finite update frequency) - if (std::isfinite(stepperUpdatePeriod_)) - { - auto systemIt = systems_.begin(); - auto systemDataIt = systemDataVec_.begin(); - for (; systemIt != systems_.end(); ++systemIt, ++systemDataIt) - { - for (auto & profileForce : systemDataIt->profileForces) - { - if (profileForce.updatePeriod > EPS) - { - double forceUpdatePeriod = profileForce.updatePeriod; - double dtNextForceUpdatePeriod = - forceUpdatePeriod - std::fmod(t, forceUpdatePeriod); - if (dtNextForceUpdatePeriod < SIMULATION_MIN_TIMESTEP || - forceUpdatePeriod - dtNextForceUpdatePeriod < STEPPER_MIN_TIMESTEP) - { - const Eigen::VectorXd & q = systemDataIt->state.q; - const Eigen::VectorXd & v = systemDataIt->state.v; - profileForce.force = profileForce.func(t, q, v); - hasDynamicsChanged = true; - } - } - } - } - } - - // Update the controller command if necessary (only for finite update frequency) - if (std::isfinite(stepperUpdatePeriod_) && - engineOptions_->stepper.controllerUpdatePeriod > EPS) - { - double controllerUpdatePeriod = engineOptions_->stepper.controllerUpdatePeriod; - double dtNextControllerUpdatePeriod = - controllerUpdatePeriod - std::fmod(t, controllerUpdatePeriod); - if (dtNextControllerUpdatePeriod < SIMULATION_MIN_TIMESTEP || - controllerUpdatePeriod - dtNextControllerUpdatePeriod < STEPPER_MIN_TIMESTEP) - { - auto systemIt = systems_.begin(); - auto systemDataIt = systemDataVec_.begin(); - for (; systemIt != systems_.end(); ++systemIt, ++systemDataIt) - { - const Eigen::VectorXd & q = systemDataIt->state.q; - const Eigen::VectorXd & v = systemDataIt->state.v; - Eigen::VectorXd & command = systemDataIt->state.command; - computeCommand(*systemIt, t, q, v, command); - } - hasDynamicsChanged = true; - } - } - - /* Update telemetry if necessary. It monitors the current iteration number, the current - time, and the systems state, command, and sensors data. - - Note that the acceleration is discontinuous. In particular, it would have different - values of the same timestep if the command has been updated. There is no way to log - both the acceleration at the end of the previous step (t-) and at the beginning of - the next one (t+). Logging the previous acceleration is more natural since it - preserves the consistency between sensors data and robot state. */ - if (!std::isfinite(stepperUpdatePeriod_) || - !engineOptions_->stepper.logInternalStepperSteps) - { - bool mustUpdateTelemetry = !std::isfinite(stepperUpdatePeriod_); - if (!mustUpdateTelemetry) - { - double dtNextStepperUpdatePeriod = - stepperUpdatePeriod_ - std::fmod(t, stepperUpdatePeriod_); - mustUpdateTelemetry = - (dtNextStepperUpdatePeriod < SIMULATION_MIN_TIMESTEP || - stepperUpdatePeriod_ - dtNextStepperUpdatePeriod < STEPPER_MIN_TIMESTEP); - } - if (mustUpdateTelemetry) - { - updateTelemetry(); - } - } - - // Fix the FSAL issue if the dynamics has changed - if (!std::isfinite(stepperUpdatePeriod_) && hasDynamicsChanged) - { - computeSystemsDynamics(t, qSplit, vSplit, aSplit, true); - syncAllAccelerationsAndForces(systems_, contactForcesPrev_, fPrev_, aPrev_); - syncSystemsStateWithStepper(true); - hasDynamicsChanged = false; - } - - if (std::isfinite(stepperUpdatePeriod_)) - { - /* Get the time of the next breakpoint for the ODE solver: a breakpoint occurs if: - - tEnd has been reached - - an external impulse force must be activated/deactivated - - the sensor measurements or the controller command must be updated. */ - double dtNextGlobal; // dt to apply for the next stepper step - const double dtNextUpdatePeriod = - stepperUpdatePeriod_ - std::fmod(t, stepperUpdatePeriod_); - if (dtNextUpdatePeriod < SIMULATION_MIN_TIMESTEP) - { - /* Step to reach next sensors/controller update is too short: skip one - controller update and jump to the next one. - - Note that in this case, the sensors have already been updated in advance - during the previous loop. */ - dtNextGlobal = - std::min(dtNextUpdatePeriod + stepperUpdatePeriod_, tImpulseForceNext - t); - } - else - { - dtNextGlobal = std::min(dtNextUpdatePeriod, tImpulseForceNext - t); - } - - /* Check if the next dt to about equal to the time difference between the current - time (it can only be smaller) and enforce next dt to exactly match this value in - such a case. */ - if (tEnd - t - STEPPER_MIN_TIMESTEP < dtNextGlobal) - { - dtNextGlobal = tEnd - t; - } - - // Update next dt - tNext += dtNextGlobal; - - // Compute the next step using adaptive step method - while (tNext - t > STEPPER_MIN_TIMESTEP) - { - // Log every stepper state only if the user asked for - if (successiveIterFailed == 0 && - engineOptions_->stepper.logInternalStepperSteps) - { - updateTelemetry(); - } - - // Fix the FSAL issue if the dynamics has changed - if (hasDynamicsChanged) - { - computeSystemsDynamics(t, qSplit, vSplit, aSplit, true); - syncAllAccelerationsAndForces( - systems_, contactForcesPrev_, fPrev_, aPrev_); - syncSystemsStateWithStepper(true); - hasDynamicsChanged = false; - } - - /* Adjust stepsize to end up exactly at the next breakpoint if it is reasonable - to expect that integration will be successful, namely: - - If the next breakpoint is closer than the estimated maximum step size - OR - - If the next breakpoint is farther but not so far away compared to the - estimated maximum step size, AND the previous integration trial was - successful. This last condition is essential to prevent infinite loop of - slightly increasing the step size, failing to integrate, then try again - and again until triggering maximum successive iteration failure exception. - The current implementation is conservative and does not check that the - previous failure was due to this stepsize adjustment procedure, but it is - just a performance optimization trick, so it should not be a big deal. */ - const double dtResidualThr = - std::min(SIMULATION_MIN_TIMESTEP, 0.1 * dtLargest); - if (tNext - t < dt || - (successiveIterFailed == 0 && tNext - t < dt + dtResidualThr)) - { - dt = tNext - t; - } - - /* Trying to reach multiples of STEPPER_MIN_TIMESTEP whenever possible. The - idea here is to reach only multiples of 1us, making logging easier, given - that, 1us can be consider an 'infinitesimal' time in robotics. This - arbitrary threshold many not be suited for simulating different, faster - dynamics, that require sub-microsecond precision. */ - if (dt > SIMULATION_MIN_TIMESTEP) - { - const double dtResidual = std::fmod(dt, SIMULATION_MIN_TIMESTEP); - if (dtResidual > STEPPER_MIN_TIMESTEP && - dtResidual < SIMULATION_MIN_TIMESTEP - STEPPER_MIN_TIMESTEP && - dt - dtResidual > STEPPER_MIN_TIMESTEP) - { - dt -= dtResidual; - } - } - - // Break the loop if dt is getting too small. The error code will be set later. - if (dt < STEPPER_MIN_TIMESTEP) - { - break; - } - - // Break the loop in case of timeout. The error code will be set later. - if (EPS < engineOptions_->stepper.timeout && - engineOptions_->stepper.timeout < timer_.toc()) - { - break; - } - - // Break the loop in case of too many successive failed inner iteration - if (successiveIterFailed > engineOptions_->stepper.successiveIterFailedMax) - { - break; - } - - /* Backup current number of successive constraint solving failure. - It will be restored in case of integration failure. */ - auto systemDataIt = systemDataVec_.begin(); - auto successiveSolveFailedIt = successiveSolveFailedAll.begin(); - for (; systemDataIt != systemDataVec_.end(); - ++systemDataIt, ++successiveSolveFailedIt) - { - *successiveSolveFailedIt = systemDataIt->successiveSolveFailed; - } - - // Break the loop in case of too many successive constraint solving failures - for (uint32_t successiveSolveFailed : successiveSolveFailedAll) - { - if (successiveSolveFailed > - engineOptions_->stepper.successiveIterFailedMax) - { - break; - } - } - - /* A breakpoint has been reached, causing dt to be smaller that necessary for - prescribed integration tol. */ - isBreakpointReached = (dtLargest > dt); - - // Set the timestep to be tried by the stepper - dtLargest = dt; - - // Try doing one integration step - bool isStepSuccessful = - stepper_->tryStep(qSplit, vSplit, aSplit, t, dtLargest); - - /* Check if the integrator failed miserably even if successfully. - It would happen the timestep is fixed and too large, causing the integrator - to fail miserably returning nan. */ - isNan = std::isnan(dtLargest); - if (isNan) - { - break; - } - - // Update buffer if really successful - if (isStepSuccessful) - { - // Reset successive iteration failure counter - successiveIterFailed = 0; - - // Synchronize the position, velocity and acceleration of all systems - syncSystemsStateWithStepper(); - - /* Compute all external terms including joints accelerations and forces. - Note that it is possible to call this method because `pinocchio::Data` - is guaranteed to be up-to-date at this point. */ - computeAllExtraTerms(systems_, systemDataVec_, fPrev_); - - // Backend the updated joint accelerations and forces - syncAllAccelerationsAndForces( - systems_, contactForcesPrev_, fPrev_, aPrev_); - - // Increment the iteration counter only for successful steps - ++stepperState_.iter; - - /* Restore the step size dt if it has been significantly decreased because - of a breakpoint. It is set equal to the last available largest dt to be - known, namely the second to last successful step. */ - if (isBreakpointReached) - { - /* Restore the step size if and only if: - - the next estimated largest step size is larger than the requested - one for the current (successful) step. - - the next estimated largest step size is significantly smaller than - the estimated largest step size for the previous step. */ - double dtRestoreThresholdAbs = - stepperState_.dtLargestPrev * - engineOptions_->stepper.dtRestoreThresholdRel; - if (dt < dtLargest && dtLargest < dtRestoreThresholdAbs) - { - dtLargest = stepperState_.dtLargestPrev; - } - } - - /* Backup the stepper and systems' state on success only: - - t at last successful iteration is used to compute dt, which is project - the acceleration in the state space instead of SO3^2. - - dtLargestPrev is used to restore the largest step size in case of a - breakpoint requiring lowering it. - - the acceleration and effort at the last successful iteration is used - to update the sensors' data in case of continuous sensing. */ - stepperState_.tPrev = t; - stepperState_.dtLargestPrev = dtLargest; - for (auto & systemData : systemDataVec_) - { - systemData.statePrev = systemData.state; - } - } - else - { - // Increment the failed iteration counters - ++successiveIterFailed; - ++stepperState_.iterFailed; - - // Restore number of successive constraint solving failure - systemDataIt = systemDataVec_.begin(); - successiveSolveFailedIt = successiveSolveFailedAll.begin(); - for (; systemDataIt != systemDataVec_.end(); - ++systemDataIt, ++successiveSolveFailedIt) - { - systemDataIt->successiveSolveFailed = *successiveSolveFailedIt; - } - } - - // Initialize the next dt - dt = std::min(dtLargest, engineOptions_->stepper.dtMax); - } - } - else - { - /* Make sure it ends exactly at the tEnd, never exceeds dtMax, and stop to apply - impulse forces. */ - dt = std::min({dt, tEnd - t, tImpulseForceNext - t}); - - /* A breakpoint has been reached, because dt has been decreased wrt the largest - possible dt within integration tol. */ - isBreakpointReached = (dtLargest > dt); - - // Compute the next step using adaptive step method - bool isStepSuccessful = false; - while (!isStepSuccessful) - { - // Set the timestep to be tried by the stepper - dtLargest = dt; - - // Break the loop in case of too many successive failed inner iteration - if (successiveIterFailed > engineOptions_->stepper.successiveIterFailedMax) - { - break; - } - - /* Backup current number of successive constraint solving failure. - It will be restored in case of integration failure. */ - auto systemDataIt = systemDataVec_.begin(); - auto successiveSolveFailedIt = successiveSolveFailedAll.begin(); - for (; systemDataIt != systemDataVec_.end(); - ++systemDataIt, ++successiveSolveFailedIt) - { - *successiveSolveFailedIt = systemDataIt->successiveSolveFailed; - } - - // Break the loop in case of too many successive constraint solving failures - for (uint32_t successiveSolveFailed : successiveSolveFailedAll) - { - if (successiveSolveFailed > - engineOptions_->stepper.successiveIterFailedMax) - { - break; - } - } - - // Try to do a step - isStepSuccessful = stepper_->tryStep(qSplit, vSplit, aSplit, t, dtLargest); - - // Check if the integrator failed miserably even if successfully - isNan = std::isnan(dtLargest); - if (isNan) - { - break; - } - - if (isStepSuccessful) - { - // Reset successive iteration failure counter - successiveIterFailed = 0; - - // Synchronize the position, velocity and acceleration of all systems - syncSystemsStateWithStepper(); - - // Compute all external terms including joints accelerations and forces - computeAllExtraTerms(systems_, systemDataVec_, fPrev_); - - // Backend the updated joint accelerations and forces - syncAllAccelerationsAndForces( - systems_, contactForcesPrev_, fPrev_, aPrev_); - - // Increment the iteration counter - ++stepperState_.iter; - - // Restore the step size if necessary - if (isBreakpointReached) - { - double dtRestoreThresholdAbs = - stepperState_.dtLargestPrev * - engineOptions_->stepper.dtRestoreThresholdRel; - if (dt < dtLargest && dtLargest < dtRestoreThresholdAbs) - { - dtLargest = stepperState_.dtLargestPrev; - } - } - - // Backup the stepper and systems' state - stepperState_.tPrev = t; - stepperState_.dtLargestPrev = dtLargest; - for (auto & systemData : systemDataVec_) - { - systemData.statePrev = systemData.state; - } - } - else - { - // Increment the failed iteration counter - ++successiveIterFailed; - ++stepperState_.iterFailed; - - // Restore number of successive constraint solving failure - systemDataIt = systemDataVec_.begin(); - successiveSolveFailedIt = successiveSolveFailedAll.begin(); - for (; systemDataIt != systemDataVec_.end(); - ++systemDataIt, ++successiveSolveFailedIt) - { - systemDataIt->successiveSolveFailed = *successiveSolveFailedIt; - } - } - - // Initialize the next dt - dt = std::min(dtLargest, engineOptions_->stepper.dtMax); - } - } - - // Exception handling - if (isNan) - { - THROW_ERROR(std::runtime_error, - "Something is wrong with the physics. Aborting integration."); - } - if (successiveIterFailed > engineOptions_->stepper.successiveIterFailedMax) - { - THROW_ERROR(std::runtime_error, - "Too many successive iteration failures. Probably something " - "is going wrong with the physics. Aborting integration."); - } - for (uint32_t successiveSolveFailed : successiveSolveFailedAll) - { - if (successiveSolveFailed > engineOptions_->stepper.successiveIterFailedMax) - { - THROW_ERROR( - std::runtime_error, - "Too many successive constraint solving failures. Try increasing the " - "regularization factor. Aborting integration."); - } - } - if (dt < STEPPER_MIN_TIMESTEP) - { - THROW_ERROR(std::runtime_error, - "The internal time step is getting too small. Impossible to " - "integrate physics further in time. Aborting integration."); - } - if (EPS < engineOptions_->stepper.timeout && - engineOptions_->stepper.timeout < timer_.toc()) - { - THROW_ERROR(std::runtime_error, "Step computation timeout. Aborting integration."); - } - - // Update sensors data if necessary, namely if time-continuous or breakpoint - const double sensorsUpdatePeriod = engineOptions_->stepper.sensorsUpdatePeriod; - const double dtNextSensorsUpdatePeriod = - sensorsUpdatePeriod - std::fmod(t, sensorsUpdatePeriod); - bool mustUpdateSensors = sensorsUpdatePeriod < EPS; - if (!mustUpdateSensors) - { - mustUpdateSensors = dtNextSensorsUpdatePeriod < SIMULATION_MIN_TIMESTEP || - sensorsUpdatePeriod - dtNextSensorsUpdatePeriod < - STEPPER_MIN_TIMESTEP; - } - if (mustUpdateSensors) - { - auto systemIt = systems_.begin(); - auto systemDataIt = systemDataVec_.begin(); - for (; systemIt != systems_.end(); ++systemIt, ++systemDataIt) - { - const Eigen::VectorXd & q = systemDataIt->state.q; - const Eigen::VectorXd & v = systemDataIt->state.v; - const Eigen::VectorXd & a = systemDataIt->state.a; - const Eigen::VectorXd & uMotor = systemDataIt->state.uMotor; - const ForceVector & fext = systemDataIt->state.fExternal; - systemIt->robot->computeSensorMeasurements(t, q, v, a, uMotor, fext); - } - } - } - - /* Update the final time and dt to make sure it corresponds to the desired values and avoid - compounding of error. Anyway the user asked for a step of exactly stepSize, so he is - expecting this value to be reached. */ - t = tEnd; - } - - void EngineMultiRobot::stop() - { - // Release the lock on the robots - for (auto & systemData : systemDataVec_) - { - systemData.robotLock.reset(nullptr); - } - - // Make sure that a simulation running - if (!isSimulationRunning_) - { - return; - } - - // Log current buffer content as final point of the log data - updateTelemetry(); - - // Clear log data buffer one last time, now that the final point has been added - logData_.reset(); - - /* Reset the telemetry. - Note that calling ``stop` or `reset` does NOT clear the internal data buffer of - telemetryRecorder_. Clearing is done at init time, so that it remains accessible until - the next initialization. */ - telemetryRecorder_->reset(); - telemetryData_->reset(); - - // Update some internal flags - isSimulationRunning_ = false; - } - - void EngineMultiRobot::registerImpulseForce(const std::string & systemName, - const std::string & frameName, - double t, - double dt, - const pinocchio::Force & force) - { - if (isSimulationRunning_) - { - THROW_ERROR( - bad_control_flow, - "Simulation already running. Please stop it before registering new forces."); - } - - if (dt < STEPPER_MIN_TIMESTEP) - { - THROW_ERROR(std::invalid_argument, - "Force duration cannot be smaller than ", - STEPPER_MIN_TIMESTEP, - "s."); - } - - if (t < 0.0) - { - THROW_ERROR(std::invalid_argument, "Force application time must be positive."); - } - - if (frameName == "universe") - { - THROW_ERROR(std::invalid_argument, - "Impossible to apply external forces to the universe itself!"); - } - - // TODO: Make sure that the forces do NOT overlap while taking into account dt. - - std::ptrdiff_t systemIndex = getSystemIndex(systemName); - pinocchio::FrameIndex frameIndex = - getFrameIndex(systems_[systemIndex].robot->pinocchioModel_, frameName); - - SystemData & systemData = systemDataVec_[systemIndex]; - systemData.impulseForces.emplace_back(frameName, frameIndex, t, dt, force); - systemData.impulseForceBreakpoints.emplace(t); - systemData.impulseForceBreakpoints.emplace(t + dt); - systemData.isImpulseForceActiveVec.emplace_back(false); - } - - template - std::tuple - isGcdIncluded(const vector_aligned_t & systemDataVec, const Args &... values) - { - if (systemDataVec.empty()) - { - return isGcdIncluded(values...); - } - - const double * valueMinPtr = &INF; - auto func = [&valueMinPtr, &values...](const SystemData & systemData) - { - auto && [isIncluded, value] = isGcdIncluded( - systemData.profileForces.cbegin(), - systemData.profileForces.cend(), - [](const ProfileForce & force) -> const double & { return force.updatePeriod; }, - values...); - valueMinPtr = &(minClipped(*valueMinPtr, value)); - return isIncluded; - }; - // FIXME: Order of evaluation is not always respected with MSVC. - bool isIncluded = std::all_of(systemDataVec.begin(), systemDataVec.end(), func); - return {isIncluded, *valueMinPtr}; - } - - void EngineMultiRobot::registerProfileForce(const std::string & systemName, - const std::string & frameName, - const ProfileForceFunction & forceFunc, - double updatePeriod) - { - if (isSimulationRunning_) - { - THROW_ERROR( - bad_control_flow, - "Simulation already running. Please stop it before registering new forces."); - } - - if (frameName == "universe") - { - THROW_ERROR(std::invalid_argument, - "Impossible to apply external forces to the universe itself!"); - } - - // Get system and frame indices - std::ptrdiff_t systemIndex = getSystemIndex(systemName); - pinocchio::FrameIndex frameIndex = - getFrameIndex(systems_[systemIndex].robot->pinocchioModel_, frameName); - - // Make sure the update period is valid - if (EPS < updatePeriod && updatePeriod < SIMULATION_MIN_TIMESTEP) - { - THROW_ERROR( - std::invalid_argument, - "Cannot register external force profile with update period smaller than ", - SIMULATION_MIN_TIMESTEP, - "s. Adjust period or switch to continuous mode by setting period to zero."); - } - // Make sure the desired update period is a multiple of the stepper period - auto [isIncluded, updatePeriodMin] = - isGcdIncluded(systemDataVec_, stepperUpdatePeriod_, updatePeriod); - if (!isIncluded) - { - THROW_ERROR(std::invalid_argument, - "In discrete mode, the update period of force profiles and the " - "stepper update period (min of controller and sensor update " - "periods) must be multiple of each other."); - } - - // Set breakpoint period during the integration loop - stepperUpdatePeriod_ = updatePeriodMin; - - // Add force profile to register - SystemData & systemData = systemDataVec_[systemIndex]; - systemData.profileForces.emplace_back(frameName, frameIndex, updatePeriod, forceFunc); - } - - void EngineMultiRobot::removeImpulseForces(const std::string & systemName) - { - // Make sure that no simulation is running - if (isSimulationRunning_) - { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before removing coupling forces."); - } - - std::ptrdiff_t systemIndex = getSystemIndex(systemName); - SystemData & systemData = systemDataVec_[systemIndex]; - systemData.impulseForces.clear(); - } - - void EngineMultiRobot::removeImpulseForces() - { - // Make sure that no simulation is running - if (isSimulationRunning_) - { - THROW_ERROR(bad_control_flow, - "simulation already running. Stop it before removing coupling forces."); - } - - for (auto & systemData : systemDataVec_) - { - systemData.impulseForces.clear(); - } - } - - void EngineMultiRobot::removeProfileForces(const std::string & systemName) - { - // Make sure that no simulation is running - if (isSimulationRunning_) - { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before removing coupling forces."); - } - - - // Remove force profile from register - std::ptrdiff_t systemIndex = getSystemIndex(systemName); - SystemData & systemData = systemDataVec_[systemIndex]; - systemData.profileForces.clear(); - - // Set breakpoint period during the integration loop - // FIXME: replaced `std::get` by placeholder `_` when moving to C++26 (P2169R4) - stepperUpdatePeriod_ = - std::get<1>(isGcdIncluded(systemDataVec_, - engineOptions_->stepper.sensorsUpdatePeriod, - engineOptions_->stepper.controllerUpdatePeriod)); - } - - void EngineMultiRobot::removeProfileForces() - { - // Make sure that no simulation is running - if (isSimulationRunning_) - { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before removing coupling forces."); - } - - for (auto & systemData : systemDataVec_) - { - systemData.profileForces.clear(); - } - } - - const ImpulseForceVector & EngineMultiRobot::getImpulseForces( - const std::string & systemName) const - { - std::ptrdiff_t systemIndex = getSystemIndex(systemName); - const SystemData & systemData = systemDataVec_[systemIndex]; - return systemData.impulseForces; - } - - const ProfileForceVector & EngineMultiRobot::getProfileForces( - const std::string & systemName) const - { - std::ptrdiff_t systemIndex = getSystemIndex(systemName); - const SystemData & systemData = systemDataVec_[systemIndex]; - return systemData.profileForces; - } - - GenericConfig EngineMultiRobot::getOptions() const noexcept - { - return engineOptionsGeneric_; - } - - void EngineMultiRobot::setOptions(const GenericConfig & engineOptions) - { - if (isSimulationRunning_) - { - THROW_ERROR(bad_control_flow, - "Simulation already running. Please stop it before updating the options."); - } - - // Make sure the dtMax is not out of range - GenericConfig stepperOptions = boost::get(engineOptions.at("stepper")); - const double dtMax = boost::get(stepperOptions.at("dtMax")); - if (SIMULATION_MAX_TIMESTEP + EPS < dtMax || dtMax < SIMULATION_MIN_TIMESTEP) - { - THROW_ERROR(std::invalid_argument, - "'dtMax' option must bge in range [", - SIMULATION_MIN_TIMESTEP, - ", ", - SIMULATION_MAX_TIMESTEP, - "]."); - } - - // Make sure successiveIterFailedMax is strictly positive - const uint32_t successiveIterFailedMax = - boost::get(stepperOptions.at("successiveIterFailedMax")); - if (successiveIterFailedMax < 1) - { - THROW_ERROR(std::invalid_argument, - "'successiveIterFailedMax' must be strictly positive."); - } - - // Make sure the selected ode solver is available and instantiate it - const std::string & odeSolver = boost::get(stepperOptions.at("odeSolver")); - if (STEPPERS.find(odeSolver) == STEPPERS.end()) - { - THROW_ERROR( - std::invalid_argument, "Requested ODE solver '", odeSolver, "' not available."); - } - - // Make sure the controller and sensor update periods are valid - const double sensorsUpdatePeriod = - boost::get(stepperOptions.at("sensorsUpdatePeriod")); - const double controllerUpdatePeriod = - boost::get(stepperOptions.at("controllerUpdatePeriod")); - auto [isIncluded, updatePeriodMin] = - isGcdIncluded(systemDataVec_, controllerUpdatePeriod, sensorsUpdatePeriod); - if ((EPS < sensorsUpdatePeriod && sensorsUpdatePeriod < SIMULATION_MIN_TIMESTEP) || - (EPS < controllerUpdatePeriod && controllerUpdatePeriod < SIMULATION_MIN_TIMESTEP)) - { - THROW_ERROR( - std::invalid_argument, - "Cannot simulate a discrete system with update period smaller than ", - SIMULATION_MIN_TIMESTEP, - "s. Adjust period or switch to continuous mode by setting period to zero."); - } - else if (!isIncluded) - { - THROW_ERROR(std::invalid_argument, - "In discrete mode, the controller and sensor update " - "periods must be multiple of each other."); - } - - // Make sure the contacts options are fine - GenericConfig constraintsOptions = - boost::get(engineOptions.at("constraints")); - const std::string & constraintSolverType = - boost::get(constraintsOptions.at("solver")); - const auto constraintSolverIt = CONSTRAINT_SOLVERS_MAP.find(constraintSolverType); - if (constraintSolverIt == CONSTRAINT_SOLVERS_MAP.end()) - { - THROW_ERROR(std::invalid_argument, - "Requested constraint solver '", - constraintSolverType, - "' not available."); - } - double regularization = boost::get(constraintsOptions.at("regularization")); - if (regularization < 0.0) - { - THROW_ERROR(std::invalid_argument, - "Constraint option 'regularization' must be positive."); - } - - // Make sure the contacts options are fine - GenericConfig contactOptions = boost::get(engineOptions.at("contacts")); - const std::string & contactModel = boost::get(contactOptions.at("model")); - const auto contactModelIt = CONTACT_MODELS_MAP.find(contactModel); - if (contactModelIt == CONTACT_MODELS_MAP.end()) - { - THROW_ERROR(std::invalid_argument, "Requested contact model not available."); - } - double contactsTransitionEps = boost::get(contactOptions.at("transitionEps")); - if (contactsTransitionEps < 0.0) - { - THROW_ERROR(std::invalid_argument, "Contact option 'transitionEps' must be positive."); - } - double transitionVelocity = boost::get(contactOptions.at("transitionVelocity")); - if (transitionVelocity < EPS) - { - THROW_ERROR(std::invalid_argument, - "Contact option 'transitionVelocity' must be strictly positive."); - } - double stabilizationFreq = boost::get(contactOptions.at("stabilizationFreq")); - if (stabilizationFreq < 0.0) - { - THROW_ERROR(std::invalid_argument, - "Contact option 'stabilizationFreq' must be positive."); - } - - // Make sure the user-defined gravity force has the right dimension - GenericConfig worldOptions = boost::get(engineOptions.at("world")); - Eigen::VectorXd gravity = boost::get(worldOptions.at("gravity")); - if (gravity.size() != 6) - { - THROW_ERROR(std::invalid_argument, "The size of the gravity force vector must be 6."); - } - - /* Reset random number generators if setOptions is called for the first time, or if the - desired random seed has changed. */ - const VectorX & seedSeq = - boost::get>(stepperOptions.at("randomSeedSeq")); - if (!engineOptions_ || seedSeq.size() != engineOptions_->stepper.randomSeedSeq.size() || - (seedSeq.array() != engineOptions_->stepper.randomSeedSeq.array()).any()) - { - generator_.seed(std::seed_seq(seedSeq.data(), seedSeq.data() + seedSeq.size())); - } - - // Update the internal options - engineOptionsGeneric_ = engineOptions; - - // Create a fast struct accessor - engineOptions_ = std::make_unique(engineOptionsGeneric_); - - // Backup contact model as enum for fast check - contactModel_ = contactModelIt->second; - - // Set breakpoint period during the integration loop - stepperUpdatePeriod_ = updatePeriodMin; - } - - std::vector EngineMultiRobot::getSystemNames() const - { - std::vector systemsNames; - systemsNames.reserve(systems_.size()); - std::transform(systems_.begin(), - systems_.end(), - std::back_inserter(systemsNames), - [](const auto & sys) -> std::string { return sys.name; }); - return systemsNames; - } - - std::ptrdiff_t EngineMultiRobot::getSystemIndex(const std::string & systemName) const - { - auto systemIt = std::find_if(systems_.begin(), - systems_.end(), - [&systemName](const auto & sys) - { return (sys.name == systemName); }); - if (systemIt == systems_.end()) - { - THROW_ERROR(std::invalid_argument, - "No system with this name has been added to the engine."); - } - - return std::distance(systems_.begin(), systemIt); - } - - System & EngineMultiRobot::getSystem(const std::string & systemName) - { - auto systemIt = std::find_if(systems_.begin(), - systems_.end(), - [&systemName](const auto & sys) - { return (sys.name == systemName); }); - if (systemIt == systems_.end()) - { - THROW_ERROR(std::invalid_argument, - "No system with this name has been added to the engine."); - } - - return *systemIt; - } - - const SystemState & EngineMultiRobot::getSystemState(const std::string & systemName) const - { - std::ptrdiff_t systemIndex = getSystemIndex(systemName); - return systemDataVec_[systemIndex].state; - } - - const StepperState & EngineMultiRobot::getStepperState() const - { - return stepperState_; - } - - const bool & EngineMultiRobot::getIsSimulationRunning() const - { - return isSimulationRunning_; - } - - double EngineMultiRobot::getSimulationDurationMax() - { - return TelemetryRecorder::getLogDurationMax(getTelemetryTimeUnit()); - } - - double EngineMultiRobot::getTelemetryTimeUnit() - { - return STEPPER_MIN_TIMESTEP; - } - - // ======================================================== - // =================== Stepper utilities ================== - // ======================================================== - - void EngineMultiRobot::syncStepperStateWithSystems() - { - auto qSplitIt = stepperState_.qSplit.begin(); - auto vSplitIt = stepperState_.vSplit.begin(); - auto aSplitIt = stepperState_.aSplit.begin(); - auto systemDataIt = systemDataVec_.begin(); - for (; systemDataIt != systemDataVec_.end(); - ++systemDataIt, ++qSplitIt, ++vSplitIt, ++aSplitIt) - { - *qSplitIt = systemDataIt->state.q; - *vSplitIt = systemDataIt->state.v; - *aSplitIt = systemDataIt->state.a; - } - } - - void EngineMultiRobot::syncSystemsStateWithStepper(bool isStateUpToDate) - { - if (isStateUpToDate) - { - auto aSplitIt = stepperState_.aSplit.begin(); - auto systemDataIt = systemDataVec_.begin(); - for (; systemDataIt != systemDataVec_.end(); ++systemDataIt, ++aSplitIt) - { - systemDataIt->state.a = *aSplitIt; - } - } - else - { - auto qSplitIt = stepperState_.qSplit.begin(); - auto vSplitIt = stepperState_.vSplit.begin(); - auto aSplitIt = stepperState_.aSplit.begin(); - auto systemDataIt = systemDataVec_.begin(); - for (; systemDataIt != systemDataVec_.end(); - ++systemDataIt, ++qSplitIt, ++vSplitIt, ++aSplitIt) - { - systemDataIt->state.q = *qSplitIt; - systemDataIt->state.v = *vSplitIt; - systemDataIt->state.a = *aSplitIt; - } - } - } - - // ======================================================== - // ================ Core physics utilities ================ - // ======================================================== - - - void EngineMultiRobot::computeForwardKinematics(System & system, - const Eigen::VectorXd & q, - const Eigen::VectorXd & v, - const Eigen::VectorXd & a) - { - // Create proxies for convenience - const pinocchio::Model & model = system.robot->pinocchioModel_; - pinocchio::Data & data = system.robot->pinocchioData_; - const pinocchio::GeometryModel & geomModel = system.robot->collisionModel_; - pinocchio::GeometryData & geomData = system.robot->collisionData_; - - // Update forward kinematics - pinocchio::forwardKinematics(model, data, q, v, a); - - // Update frame placements (avoiding redundant computations) - for (pinocchio::FrameIndex frameIndex = 1; - frameIndex < static_cast(model.nframes); - ++frameIndex) - { - const pinocchio::Frame & frame = model.frames[frameIndex]; - pinocchio::JointIndex parentJointIndex = frame.parent; - switch (frame.type) - { - case pinocchio::FrameType::JOINT: - /* If the frame is associated with an actual joint, no need to compute anything - new, since the frame transform is supposed to be identity. */ - data.oMf[frameIndex] = data.oMi[parentJointIndex]; - break; - case pinocchio::FrameType::BODY: - if (model.frames[frame.previousFrame].type == pinocchio::FrameType::FIXED_JOINT) - { - /* BODYs connected via FIXED_JOINT(s) have the same transform than the joint - itself, so no need to compute them twice. Here we are doing the assumption - that the previous frame transform has already been updated since it is - closer to root in kinematic tree. */ - data.oMf[frameIndex] = data.oMf[frame.previousFrame]; - } - else - { - /* BODYs connected via JOINT(s) have the identity transform, so copying parent - joint transform should be fine. */ - data.oMf[frameIndex] = data.oMi[parentJointIndex]; - } - break; - case pinocchio::FrameType::FIXED_JOINT: - case pinocchio::FrameType::SENSOR: - case pinocchio::FrameType::OP_FRAME: - default: - // Nothing special, doing the actual computation - data.oMf[frameIndex] = data.oMi[parentJointIndex] * frame.placement; - } - } - - /* Update collision information selectively, ie only for geometries involved in at least - one collision pair. */ - pinocchio::updateGeometryPlacements(model, data, geomModel, geomData); - pinocchio::computeCollisions(geomModel, geomData, false); - } - - void EngineMultiRobot::computeContactDynamicsAtBody( - const System & system, - const pinocchio::PairIndex & collisionPairIndex, - std::shared_ptr & constraint, - pinocchio::Force & fextLocal) const - { - // TODO: It is assumed that the ground is flat. For now ground profile is not supported - // with body collision. Nevertheless it should not be to hard to generated a collision - // object simply by sampling points on the profile. - - // Define proxy for convenience - pinocchio::Data & data = system.robot->pinocchioData_; - - // Get the frame and joint indices - const pinocchio::GeomIndex & geometryIndex = - system.robot->collisionModel_.collisionPairs[collisionPairIndex].first; - pinocchio::JointIndex parentJointIndex = - system.robot->collisionModel_.geometryObjects[geometryIndex].parentJoint; - - // Extract collision and distance results - const hpp::fcl::CollisionResult & collisionResult = - system.robot->collisionData_.collisionResults[collisionPairIndex]; - - fextLocal.setZero(); - - /* There is no way to get access to the distance from the ground at this point, so it is - not possible to disable the constraint only if depth > transitionEps. */ - if (constraint) - { - constraint->disable(); - } - - for (std::size_t contactIndex = 0; contactIndex < collisionResult.numContacts(); - ++contactIndex) - { - /* Extract the contact information. - Note that there is always a single contact point while computing the collision - between two shape objects, for instance convex geometry and box primitive. */ - const auto & contact = collisionResult.getContact(contactIndex); - Eigen::Vector3d nGround = contact.normal.normalized(); // Ground normal in world frame - double depth = contact.penetration_depth; // Penetration depth (signed, negative) - pinocchio::SE3 posContactInWorld = pinocchio::SE3::Identity(); - // TODO double check that it may be between both interfaces - posContactInWorld.translation() = contact.pos; // Point inside the ground - - /* FIXME: Make sure the collision computation didn't failed. If it happens the norm of - the distance normal is not normalized (usually close to zero). If so, just assume - there is no collision at all. */ - if (nGround.norm() < 1.0 - EPS) - { - continue; - } - - // Make sure the normal is always pointing upward and the penetration depth is negative - if (nGround[2] < 0.0) - { - nGround *= -1.0; - } - if (depth > 0.0) - { - depth *= -1.0; - } - - if (contactModel_ == ContactModelType::SPRING_DAMPER) - { - // Compute the linear velocity of the contact point in world frame - const pinocchio::Motion & motionJointLocal = data.v[parentJointIndex]; - const pinocchio::SE3 & transformJointFrameInWorld = data.oMi[parentJointIndex]; - const pinocchio::SE3 transformJointFrameInContact = - posContactInWorld.actInv(transformJointFrameInWorld); - const Eigen::Vector3d vContactInWorld = - transformJointFrameInContact.act(motionJointLocal).linear(); - - // Compute the ground reaction force at contact point in world frame - const pinocchio::Force fextAtContactInGlobal = - computeContactDynamics(nGround, depth, vContactInWorld); - - // Move the force at parent frame location - fextLocal += transformJointFrameInContact.actInv(fextAtContactInGlobal); - } - else - { - // The constraint is not initialized for geometry shapes that are not supported - if (constraint) - { - // In case of slippage the contact point has actually moved and must be updated - constraint->enable(); - auto & frameConstraint = static_cast(*constraint.get()); - const pinocchio::FrameIndex frameIndex = frameConstraint.getFrameIndex(); - frameConstraint.setReferenceTransform( - {data.oMf[frameIndex].rotation(), - data.oMf[frameIndex].translation() - depth * nGround}); - frameConstraint.setNormal(nGround); - - // Only one contact constraint per collision body is supported for now - break; - } - } - } - } - - void EngineMultiRobot::computeContactDynamicsAtFrame( - const System & system, - pinocchio::FrameIndex frameIndex, - std::shared_ptr & constraint, - pinocchio::Force & fextLocal) const - { - /* Returns the external force in the contact frame. It must then be converted into a force - onto the parent joint. - /!\ Note that the contact dynamics depends only on kinematics data. /!\ */ - - // Define proxies for convenience - const pinocchio::Model & model = system.robot->pinocchioModel_; - const pinocchio::Data & data = system.robot->pinocchioData_; - - // Get the pose of the frame wrt the world - const pinocchio::SE3 & transformFrameInWorld = data.oMf[frameIndex]; - - // Compute the ground normal and penetration depth at the contact point - double heightGround; - Eigen::Vector3d normalGround; - const Eigen::Vector3d & posFrame = transformFrameInWorld.translation(); - engineOptions_->world.groundProfile(posFrame.head<2>(), heightGround, normalGround); - normalGround.normalize(); // Make sure the ground normal is normalized - - // First-order projection (exact assuming no curvature) - const double depth = (posFrame[2] - heightGround) * normalGround[2]; - - // Only compute the ground reaction force if the penetration depth is negative - if (depth < 0.0) - { - // Apply the force at the origin of the parent joint frame - if (contactModel_ == ContactModelType::SPRING_DAMPER) - { - // Compute the linear velocity of the contact point in world frame - const Eigen::Vector3d motionFrameLocal = - pinocchio::getFrameVelocity(model, data, frameIndex).linear(); - const Eigen::Matrix3d & rotFrame = transformFrameInWorld.rotation(); - const Eigen::Vector3d vContactInWorld = rotFrame * motionFrameLocal; - - // Compute the ground reaction force in world frame (local world aligned) - const pinocchio::Force fextAtContactInGlobal = - computeContactDynamics(normalGround, depth, vContactInWorld); - - // Deduce the ground reaction force in joint frame - fextLocal = - convertForceGlobalFrameToJoint(model, data, frameIndex, fextAtContactInGlobal); - } - else - { - // Enable fixed frame constraint - constraint->enable(); - } - } - else - { - if (contactModel_ == ContactModelType::SPRING_DAMPER) - { - // Not in contact with the ground, thus no force applied - fextLocal.setZero(); - } - else if (depth > engineOptions_->contacts.transitionEps) - { - /* Disable fixed frame constraint only if the frame move higher `transitionEps` to - avoid sporadic contact detection. */ - constraint->disable(); - } - } - - /* Move the reference position at the surface of the ground. - Note that it is must done systematically as long as the constraint is enabled because in - case of slippage the contact point has actually moved. */ - if (constraint->getIsEnabled()) - { - auto & frameConstraint = static_cast(*constraint.get()); - frameConstraint.setReferenceTransform( - {transformFrameInWorld.rotation(), posFrame - depth * normalGround}); - frameConstraint.setNormal(normalGround); - } - } - - pinocchio::Force EngineMultiRobot::computeContactDynamics( - const Eigen::Vector3d & normalGround, - double depth, - const Eigen::Vector3d & vContactInWorld) const - { - // Initialize the contact force - Eigen::Vector3d fextInWorld; - - if (depth < 0.0) - { - // Extract some proxies - const ContactOptions & contactOptions_ = engineOptions_->contacts; - - // Compute the penetration speed - const double vDepth = vContactInWorld.dot(normalGround); - - // Compute normal force - const double fextNormal = -std::min( - contactOptions_.stiffness * depth + contactOptions_.damping * vDepth, 0.0); - fextInWorld.noalias() = fextNormal * normalGround; - - // Compute friction forces - const Eigen::Vector3d vTangential = vContactInWorld - vDepth * normalGround; - const double vRatio = - std::min(vTangential.norm() / contactOptions_.transitionVelocity, 1.0); - const double fextTangential = contactOptions_.friction * vRatio * fextNormal; - fextInWorld.noalias() -= fextTangential * vTangential; - - // Add blending factor - if (contactOptions_.transitionEps > EPS) - { - const double blendingFactor = -depth / contactOptions_.transitionEps; - const double blendingLaw = std::tanh(2.0 * blendingFactor); - fextInWorld *= blendingLaw; - } - } - else - { - fextInWorld.setZero(); - } - - return {fextInWorld, Eigen::Vector3d::Zero()}; - } - - void EngineMultiRobot::computeCommand(System & system, - double t, - const Eigen::VectorXd & q, - const Eigen::VectorXd & v, - Eigen::VectorXd & command) - { - // Reinitialize the external forces - command.setZero(); - - // Command the command - system.controller->computeCommand(t, q, v, command); - } - - template class JointModel, typename Scalar, int Options, int axis> - static std::enable_if_t< - is_pinocchio_joint_revolute_v> || - is_pinocchio_joint_revolute_unbounded_v>, - double> - getSubtreeInertiaProj(const JointModel & /* model */, - const pinocchio::Inertia & Isubtree) - { - double inertiaProj = Isubtree.inertia()(axis, axis); - for (Eigen::Index i = 0; i < 3; ++i) - { - if (i != axis) - { - inertiaProj += Isubtree.mass() * std::pow(Isubtree.lever()[i], 2); - } - } - return inertiaProj; - } - - template - static std::enable_if_t || - is_pinocchio_joint_revolute_unbounded_unaligned_v, - double> - getSubtreeInertiaProj(const JointModel & model, const pinocchio::Inertia & Isubtree) - { - return model.axis.dot(Isubtree.inertia() * model.axis) + - Isubtree.mass() * model.axis.cross(Isubtree.lever()).squaredNorm(); - } - - template - static std::enable_if_t || - is_pinocchio_joint_prismatic_unaligned_v, - double> - getSubtreeInertiaProj(const JointModel & /* model */, const pinocchio::Inertia & Isubtree) - { - return Isubtree.mass(); - } - - struct computePositionLimitsForcesAlgo : - public pinocchio::fusion::JointUnaryVisitorBase - { - typedef boost::fusion::vector< - const pinocchio::Data & /* pinocchioData */, - const Eigen::VectorXd & /* q */, - const Eigen::VectorXd & /* v */, - const Eigen::VectorXd & /* positionLimitMin */, - const Eigen::VectorXd & /* positionLimitMax */, - const std::unique_ptr & /* engineOptions */, - ContactModelType /* contactModel */, - std::shared_ptr & /* constraint */, - Eigen::VectorXd & /* u */> - ArgsType; - - template - static std::enable_if_t || - is_pinocchio_joint_revolute_unaligned_v || - is_pinocchio_joint_prismatic_v || - is_pinocchio_joint_prismatic_unaligned_v, - void> - algo(const pinocchio::JointModelBase & joint, - const pinocchio::Data & data, - const Eigen::VectorXd & q, - const Eigen::VectorXd & v, - const Eigen::VectorXd & positionLimitMin, - const Eigen::VectorXd & positionLimitMax, - const std::unique_ptr & engineOptions, - ContactModelType contactModel, - std::shared_ptr & constraint, - Eigen::VectorXd & u) - { - // Define some proxies for convenience - const pinocchio::JointIndex jointIndex = joint.id(); - const Eigen::Index positionIndex = joint.idx_q(); - const Eigen::Index velocityIndex = joint.idx_v(); - const double qJoint = q[positionIndex]; - const double qJointMin = positionLimitMin[positionIndex]; - const double qJointMax = positionLimitMax[positionIndex]; - const double vJoint = v[velocityIndex]; - const double Ia = getSubtreeInertiaProj(joint.derived(), data.Ycrb[jointIndex]); - const double stiffness = engineOptions->joints.boundStiffness; - const double damping = engineOptions->joints.boundDamping; - const double transitionEps = engineOptions->contacts.transitionEps; - - // Check if out-of-bounds - if (contactModel == ContactModelType::SPRING_DAMPER) - { - // Compute the acceleration to apply to move out of the bound - double accelJoint = 0.0; - if (qJoint > qJointMax) - { - const double qJointError = qJoint - qJointMax; - accelJoint = -std::max(stiffness * qJointError + damping * vJoint, 0.0); - } - else if (qJoint < qJointMin) - { - const double qJointError = qJoint - qJointMin; - accelJoint = -std::min(stiffness * qJointError + damping * vJoint, 0.0); - } - - // Apply the resulting force - u[velocityIndex] += Ia * accelJoint; - } - else - { - if (qJointMax < qJoint || qJoint < qJointMin) - { - // Enable fixed joint constraint and reset it if it was disable - constraint->enable(); - auto & jointConstraint = static_cast(*constraint.get()); - jointConstraint.setReferenceConfiguration( - Eigen::Matrix(std::clamp(qJoint, qJointMin, qJointMax))); - jointConstraint.setRotationDir(qJointMax < qJoint); - } - else if (qJointMin + transitionEps < qJoint && qJoint < qJointMax - transitionEps) - { - // Disable fixed joint constraint - constraint->disable(); - } - } - } - - template - static std::enable_if_t || - is_pinocchio_joint_revolute_unbounded_unaligned_v, - void> - algo(const pinocchio::JointModelBase & /* joint */, - const pinocchio::Data & /* data */, - const Eigen::VectorXd & /* q */, - const Eigen::VectorXd & /* v */, - const Eigen::VectorXd & /* positionLimitMin */, - const Eigen::VectorXd & /* positionLimitMax */, - const std::unique_ptr & /* engineOptions */, - ContactModelType contactModel, - std::shared_ptr & constraint, - Eigen::VectorXd & /* u */) - { - if (contactModel == ContactModelType::CONSTRAINT) - { - // Disable fixed joint constraint - constraint->disable(); - } - } - - template - static std::enable_if_t || - is_pinocchio_joint_spherical_v || - is_pinocchio_joint_spherical_zyx_v || - is_pinocchio_joint_translation_v || - is_pinocchio_joint_planar_v || - is_pinocchio_joint_mimic_v || - is_pinocchio_joint_composite_v, - void> - algo(const pinocchio::JointModelBase & /* joint */, - const pinocchio::Data & /* data */, - const Eigen::VectorXd & /* q */, - const Eigen::VectorXd & /* v */, - const Eigen::VectorXd & /* positionLimitMin */, - const Eigen::VectorXd & /* positionLimitMax */, - const std::unique_ptr & /* engineOptions */, - ContactModelType contactModel, - std::shared_ptr & constraint, - Eigen::VectorXd & /* u */) - { - PRINT_WARNING("No position bounds implemented for this type of joint."); - if (contactModel == ContactModelType::CONSTRAINT) - { - // Disable fixed joint constraint - constraint->disable(); - } - } - }; - - struct computeVelocityLimitsForcesAlgo : - public pinocchio::fusion::JointUnaryVisitorBase - { - typedef boost::fusion::vector< - const pinocchio::Data & /* data */, - const Eigen::VectorXd & /* v */, - const Eigen::VectorXd & /* velocityLimitMax */, - const std::unique_ptr & /* engineOptions */, - ContactModelType /* contactModel */, - Eigen::VectorXd & /* u */> - ArgsType; - template - static std::enable_if_t || - is_pinocchio_joint_revolute_unaligned_v || - is_pinocchio_joint_revolute_unbounded_v || - is_pinocchio_joint_revolute_unbounded_unaligned_v || - is_pinocchio_joint_prismatic_v || - is_pinocchio_joint_prismatic_unaligned_v, - void> - algo(const pinocchio::JointModelBase & joint, - const pinocchio::Data & data, - const Eigen::VectorXd & v, - const Eigen::VectorXd & velocityLimitMax, - const std::unique_ptr & engineOptions, - ContactModelType contactModel, - Eigen::VectorXd & u) - { - // Define some proxies for convenience - const pinocchio::JointIndex jointIndex = joint.id(); - const Eigen::Index velocityIndex = joint.idx_v(); - const double vJoint = v[velocityIndex]; - const double vJointMin = -velocityLimitMax[velocityIndex]; - const double vJointMax = velocityLimitMax[velocityIndex]; - const double Ia = getSubtreeInertiaProj(joint.derived(), data.Ycrb[jointIndex]); - const double damping = engineOptions->joints.boundDamping; - - // Check if out-of-bounds - if (contactModel == ContactModelType::SPRING_DAMPER) - { - // Compute joint velocity error - double vJointError = 0.0; - if (vJoint > vJointMax) - { - vJointError = vJoint - vJointMax; - } - else if (vJoint < vJointMin) - { - vJointError = vJoint - vJointMin; - } - else - { - return; - } - - // Generate acceleration in the opposite direction if out-of-bounds - const double accelJoint = -2.0 * damping * vJointError; - - // Apply the resulting force - u[velocityIndex] += Ia * accelJoint; - } - } - - template - static std::enable_if_t || - is_pinocchio_joint_spherical_v || - is_pinocchio_joint_spherical_zyx_v || - is_pinocchio_joint_translation_v || - is_pinocchio_joint_planar_v || - is_pinocchio_joint_mimic_v || - is_pinocchio_joint_composite_v, - void> - algo(const pinocchio::JointModelBase & /* joint */, - const pinocchio::Data & /* data */, - const Eigen::VectorXd & /* v */, - const Eigen::VectorXd & /* velocityLimitMax */, - const std::unique_ptr & /* engineOptions */, - ContactModelType /* contactModel */, - Eigen::VectorXd & /* u */) - { - PRINT_WARNING("No velocity bounds implemented for this type of joint."); - } - }; - - void EngineMultiRobot::computeInternalDynamics(const System & system, - SystemData & systemData, - double /* t */, - const Eigen::VectorXd & q, - const Eigen::VectorXd & v, - Eigen::VectorXd & uInternal) const - { - // Define some proxies - const pinocchio::Model & model = system.robot->pinocchioModel_; - const pinocchio::Data & data = system.robot->pinocchioData_; - - // Enforce the position limit (rigid joints only) - if (system.robot->modelOptions_->joints.enablePositionLimit) - { - const Eigen::VectorXd & positionLimitMin = system.robot->getPositionLimitMin(); - const Eigen::VectorXd & positionLimitMax = system.robot->getPositionLimitMax(); - const std::vector & rigidJointIndices = - system.robot->getRigidJointIndices(); - for (std::size_t i = 0; i < rigidJointIndices.size(); ++i) - { - auto & constraint = systemData.constraints.boundJoints[i].second; - computePositionLimitsForcesAlgo::run( - model.joints[rigidJointIndices[i]], - typename computePositionLimitsForcesAlgo::ArgsType(data, - q, - v, - positionLimitMin, - positionLimitMax, - engineOptions_, - contactModel_, - constraint, - uInternal)); - } - } - - // Enforce the velocity limit (rigid joints only) - if (system.robot->modelOptions_->joints.enableVelocityLimit) - { - const Eigen::VectorXd & velocityLimitMax = system.robot->getVelocityLimit(); - for (pinocchio::JointIndex rigidJointIndex : system.robot->getRigidJointIndices()) - { - computeVelocityLimitsForcesAlgo::run( - model.joints[rigidJointIndex], - typename computeVelocityLimitsForcesAlgo::ArgsType( - data, v, velocityLimitMax, engineOptions_, contactModel_, uInternal)); - } - } - - // Compute the flexibilities (only support JointModelType::SPHERICAL so far) - double angle; - Eigen::Matrix3d rotJlog3; - const Robot::DynamicsOptions & modelDynOptions = system.robot->modelOptions_->dynamics; - const std::vector & flexibilityJointIndices = - system.robot->getFlexibleJointIndices(); - for (std::size_t i = 0; i < flexibilityJointIndices.size(); ++i) - { - const pinocchio::JointIndex jointIndex = flexibilityJointIndices[i]; - const Eigen::Index positionIndex = model.joints[jointIndex].idx_q(); - const Eigen::Index velocityIndex = model.joints[jointIndex].idx_v(); - const Eigen::Vector3d & stiffness = modelDynOptions.flexibilityConfig[i].stiffness; - const Eigen::Vector3d & damping = modelDynOptions.flexibilityConfig[i].damping; - - const Eigen::Map quat(q.segment<4>(positionIndex).data()); - const Eigen::Vector3d angleAxis = pinocchio::quaternion::log3(quat, angle); - if (angle > 0.95 * M_PI) // Angle is always positive - { - THROW_ERROR(std::runtime_error, - "Flexible joint angle must be smaller than 0.95 * pi."); - } - pinocchio::Jlog3(angle, angleAxis, rotJlog3); - uInternal.segment<3>(velocityIndex) -= - rotJlog3 * (stiffness.array() * angleAxis.array()).matrix(); - uInternal.segment<3>(velocityIndex).array() -= - damping.array() * v.segment<3>(velocityIndex).array(); - } - } - - void EngineMultiRobot::computeCollisionForces(const System & system, - SystemData & systemData, - ForceVector & fext, - bool isStateUpToDate) const - { - // Compute the forces at contact points - const std::vector & contactFrameIndices = - system.robot->getContactFrameIndices(); - for (std::size_t i = 0; i < contactFrameIndices.size(); ++i) - { - // Compute force at the given contact frame. - const pinocchio::FrameIndex frameIndex = contactFrameIndices[i]; - auto & constraint = systemData.constraints.contactFrames[i].second; - pinocchio::Force & fextLocal = systemData.contactFrameForces[i]; - if (!isStateUpToDate) - { - computeContactDynamicsAtFrame(system, frameIndex, constraint, fextLocal); - } - - // Apply the force at the origin of the parent joint frame, in local joint frame - const pinocchio::JointIndex parentJointIndex = - system.robot->pinocchioModel_.frames[frameIndex].parent; - fext[parentJointIndex] += fextLocal; - - // Convert contact force from global frame to local frame to store it in contactForces_ - const pinocchio::SE3 & transformContactInJoint = - system.robot->pinocchioModel_.frames[frameIndex].placement; - system.robot->contactForces_[i] = transformContactInJoint.actInv(fextLocal); - } - - // Compute the force at collision bodies - const std::vector & collisionBodyIndices = - system.robot->getCollisionBodyIndices(); - const std::vector> & collisionPairIndices = - system.robot->getCollisionPairIndices(); - for (std::size_t i = 0; i < collisionBodyIndices.size(); ++i) - { - /* Compute force at the given collision body. - It returns the force applied at the origin of parent joint frame in global frame. */ - const pinocchio::FrameIndex frameIndex = collisionBodyIndices[i]; - const pinocchio::JointIndex parentJointIndex = - system.robot->pinocchioModel_.frames[frameIndex].parent; - for (std::size_t j = 0; j < collisionPairIndices[i].size(); ++j) - { - pinocchio::Force & fextLocal = systemData.collisionBodiesForces[i][j]; - if (!isStateUpToDate) - { - const pinocchio::PairIndex & collisionPairIndex = collisionPairIndices[i][j]; - auto & constraint = systemData.constraints.collisionBodies[i][j].second; - computeContactDynamicsAtBody( - system, collisionPairIndex, constraint, fextLocal); - } - - // Apply the force at the origin of the parent joint frame, in local joint frame - fext[parentJointIndex] += fextLocal; - } - } - } - - void EngineMultiRobot::computeExternalForces(const System & system, - SystemData & systemData, - double t, - const Eigen::VectorXd & q, - const Eigen::VectorXd & v, - ForceVector & fext) - { - // Add the effect of user-defined external impulse forces - auto isImpulseForceActiveIt = systemData.isImpulseForceActiveVec.begin(); - auto impulseForceIt = systemData.impulseForces.begin(); - for (; impulseForceIt != systemData.impulseForces.end(); - ++isImpulseForceActiveIt, ++impulseForceIt) - { - /* Do not check if the force is active at this point. This is managed at stepper level - to get around the ambiguous t- versus t+. */ - if (*isImpulseForceActiveIt) - { - const pinocchio::FrameIndex frameIndex = impulseForceIt->frameIndex; - const pinocchio::JointIndex parentJointIndex = - system.robot->pinocchioModel_.frames[frameIndex].parent; - fext[parentJointIndex] += - convertForceGlobalFrameToJoint(system.robot->pinocchioModel_, - system.robot->pinocchioData_, - frameIndex, - impulseForceIt->force); - } - } - - // Add the effect of time-continuous external force profiles - for (auto & profileForce : systemData.profileForces) - { - const pinocchio::FrameIndex frameIndex = profileForce.frameIndex; - const pinocchio::JointIndex parentJointIndex = - system.robot->pinocchioModel_.frames[frameIndex].parent; - if (profileForce.updatePeriod < EPS) - { - profileForce.force = profileForce.func(t, q, v); - } - fext[parentJointIndex] += convertForceGlobalFrameToJoint(system.robot->pinocchioModel_, - system.robot->pinocchioData_, - frameIndex, - profileForce.force); - } - } - - void EngineMultiRobot::computeCouplingForces(double t, - const std::vector & qSplit, - const std::vector & vSplit) - { - for (auto & couplingForce : couplingForces_) - { - // Extract info about the first system involved - const std::ptrdiff_t systemIndex1 = couplingForce.systemIndex1; - const System & system1 = systems_[systemIndex1]; - const Eigen::VectorXd & q1 = qSplit[systemIndex1]; - const Eigen::VectorXd & v1 = vSplit[systemIndex1]; - const pinocchio::FrameIndex frameIndex1 = couplingForce.frameIndex1; - ForceVector & fext1 = systemDataVec_[systemIndex1].state.fExternal; - - // Extract info about the second system involved - const std::ptrdiff_t systemIndex2 = couplingForce.systemIndex2; - const System & system2 = systems_[systemIndex2]; - const Eigen::VectorXd & q2 = qSplit[systemIndex2]; - const Eigen::VectorXd & v2 = vSplit[systemIndex2]; - const pinocchio::FrameIndex frameIndex2 = couplingForce.frameIndex2; - ForceVector & fext2 = systemDataVec_[systemIndex2].state.fExternal; - - // Compute the coupling force - pinocchio::Force force = couplingForce.func(t, q1, v1, q2, v2); - const pinocchio::JointIndex parentJointIndex1 = - system1.robot->pinocchioModel_.frames[frameIndex1].parent; - fext1[parentJointIndex1] += convertForceGlobalFrameToJoint( - system1.robot->pinocchioModel_, system1.robot->pinocchioData_, frameIndex1, force); - - // Move force from frame1 to frame2 to apply it to the second system - force.toVector() *= -1; - const pinocchio::JointIndex parentJointIndex2 = - system2.robot->pinocchioModel_.frames[frameIndex2].parent; - const Eigen::Vector3d offset = - system2.robot->pinocchioData_.oMf[frameIndex2].translation() - - system1.robot->pinocchioData_.oMf[frameIndex1].translation(); - force.angular() -= offset.cross(force.linear()); - fext2[parentJointIndex2] += convertForceGlobalFrameToJoint( - system2.robot->pinocchioModel_, system2.robot->pinocchioData_, frameIndex2, force); - } - } - - void EngineMultiRobot::computeAllTerms(double t, - const std::vector & qSplit, - const std::vector & vSplit, - bool isStateUpToDate) - { - // Reinitialize the external forces and internal efforts - for (auto & systemData : systemDataVec_) - { - for (pinocchio::Force & fext_i : systemData.state.fExternal) - { - fext_i.setZero(); - } - if (!isStateUpToDate) - { - systemData.state.uInternal.setZero(); - } - } - - // Compute the internal forces - computeCouplingForces(t, qSplit, vSplit); - - // Compute each individual system dynamics - auto systemIt = systems_.begin(); - auto systemDataIt = systemDataVec_.begin(); - auto qIt = qSplit.begin(); - auto vIt = vSplit.begin(); - for (; systemIt != systems_.end(); ++systemIt, ++systemDataIt, ++qIt, ++vIt) - { - // Define some proxies - ForceVector & fext = systemDataIt->state.fExternal; - Eigen::VectorXd & uInternal = systemDataIt->state.uInternal; - - /* Compute internal dynamics, namely the efforts in joint space associated with - position/velocity bounds dynamics, and flexibility dynamics. */ - if (!isStateUpToDate) - { - computeInternalDynamics(*systemIt, *systemDataIt, t, *qIt, *vIt, uInternal); - } - - /* Compute the collision forces and estimated time at which the contact state will - changed (Take-off / Touch-down). */ - computeCollisionForces(*systemIt, *systemDataIt, fext, isStateUpToDate); - - // Compute the external contact forces. - computeExternalForces(*systemIt, *systemDataIt, t, *qIt, *vIt, fext); - } - } - - void EngineMultiRobot::computeSystemsDynamics(double t, - const std::vector & qSplit, - const std::vector & vSplit, - std::vector & aSplit, - bool isStateUpToDate) - { - /* Note that the position of the free flyer is in world frame, whereas the velocities and - accelerations are relative to the parent body frame. */ - - // Make sure that a simulation is running - if (!isSimulationRunning_) - { - THROW_ERROR(std::logic_error, - "No simulation running. Please start one before calling this method."); - } - - // Make sure memory has been allocated for the output acceleration - aSplit.resize(vSplit.size()); - - if (!isStateUpToDate) - { - // Update kinematics for each system - auto systemIt = systems_.begin(); - auto systemDataIt = systemDataVec_.begin(); - auto qIt = qSplit.begin(); - auto vIt = vSplit.begin(); - for (; systemIt != systems_.end(); ++systemIt, ++systemDataIt, ++qIt, ++vIt) - { - const Eigen::VectorXd & aPrev = systemDataIt->statePrev.a; - computeForwardKinematics(*systemIt, *qIt, *vIt, aPrev); - } - } - - /* Compute internal and external forces and efforts applied on every systems, excluding - user-specified internal dynamics if any. - - Note that one must call this method BEFORE updating the sensors since the force sensor - measurements rely on robot_->contactForces_. */ - computeAllTerms(t, qSplit, vSplit, isStateUpToDate); - - // Compute each individual system dynamics - auto systemIt = systems_.begin(); - auto systemDataIt = systemDataVec_.begin(); - auto qIt = qSplit.begin(); - auto vIt = vSplit.begin(); - auto contactForcesPrevIt = contactForcesPrev_.begin(); - auto fPrevIt = fPrev_.begin(); - auto aPrevIt = aPrev_.begin(); - auto aIt = aSplit.begin(); - for (; systemIt != systems_.end(); ++systemIt, - ++systemDataIt, - ++qIt, - ++vIt, - ++aIt, - ++contactForcesPrevIt, - ++fPrevIt, - ++aPrevIt) - { - // Define some proxies - Eigen::VectorXd & u = systemDataIt->state.u; - Eigen::VectorXd & command = systemDataIt->state.command; - Eigen::VectorXd & uMotor = systemDataIt->state.uMotor; - Eigen::VectorXd & uInternal = systemDataIt->state.uInternal; - Eigen::VectorXd & uCustom = systemDataIt->state.uCustom; - ForceVector & fext = systemDataIt->state.fExternal; - const Eigen::VectorXd & aPrev = systemDataIt->statePrev.a; - const Eigen::VectorXd & uMotorPrev = systemDataIt->statePrev.uMotor; - const ForceVector & fextPrev = systemDataIt->statePrev.fExternal; - - /* Update the sensor data if necessary (only for infinite update frequency). - Note that it is impossible to have access to the current accelerations and efforts - since they depend on the sensor values themselves. */ - if (!isStateUpToDate && engineOptions_->stepper.sensorsUpdatePeriod < EPS) - { - // Roll back to forces and accelerations computed at previous iteration - contactForcesPrevIt->swap(systemIt->robot->contactForces_); - fPrevIt->swap(systemIt->robot->pinocchioData_.f); - aPrevIt->swap(systemIt->robot->pinocchioData_.a); - - // Update sensors based on previous accelerations and forces - systemIt->robot->computeSensorMeasurements( - t, *qIt, *vIt, aPrev, uMotorPrev, fextPrev); - - // Restore current forces and accelerations - contactForcesPrevIt->swap(systemIt->robot->contactForces_); - fPrevIt->swap(systemIt->robot->pinocchioData_.f); - aPrevIt->swap(systemIt->robot->pinocchioData_.a); - } - - /* Update the controller command if necessary (only for infinite update frequency). - Make sure that the sensor state has been updated beforehand. */ - if (engineOptions_->stepper.controllerUpdatePeriod < EPS) - { - computeCommand(*systemIt, t, *qIt, *vIt, command); - } - - /* Compute the actual motor effort. - Note that it is impossible to have access to the current accelerations. */ - systemIt->robot->computeMotorEfforts(t, *qIt, *vIt, aPrev, command); - uMotor = systemIt->robot->getMotorEfforts(); - - /* Compute the user-defined internal dynamics. - Make sure that the sensor state has been updated beforehand since the user-defined - internal dynamics may rely on it. */ - uCustom.setZero(); - systemIt->controller->internalDynamics(t, *qIt, *vIt, uCustom); - - // Compute the total effort vector - u = uInternal + uCustom; - for (const auto & motor : systemIt->robot->getMotors()) - { - const std::size_t motorIndex = motor->getIndex(); - const Eigen::Index motorVelocityIndex = motor->getJointVelocityIndex(); - u[motorVelocityIndex] += uMotor[motorIndex]; - } - - // Compute the dynamics - *aIt = computeAcceleration( - *systemIt, *systemDataIt, *qIt, *vIt, u, fext, isStateUpToDate); - } - } - - const Eigen::VectorXd & EngineMultiRobot::computeAcceleration(System & system, - SystemData & systemData, - const Eigen::VectorXd & q, - const Eigen::VectorXd & v, - const Eigen::VectorXd & u, - ForceVector & fext, - bool isStateUpToDate, - bool ignoreBounds) - { - const pinocchio::Model & model = system.robot->pinocchioModel_; - pinocchio::Data & data = system.robot->pinocchioData_; - - if (system.robot->hasConstraints()) - { - if (!isStateUpToDate) - { - // Compute kinematic constraints. It will take care of updating the joint Jacobian. - system.robot->computeConstraints(q, v); - - // Compute non-linear effects - pinocchio::nonLinearEffects(model, data, q, v); - } - - // Project external forces from cartesian space to joint space. - data.u = u; - for (int i = 1; i < model.njoints; ++i) - { - /* Skip computation if force is zero for efficiency. It should be the case most - often. */ - if ((fext[i].toVector().array().abs() > EPS).any()) - { - if (!isStateUpToDate) - { - pinocchio::getJointJacobian( - model, data, i, pinocchio::LOCAL, systemData.jointJacobians[i]); - } - data.u.noalias() += - systemData.jointJacobians[i].transpose() * fext[i].toVector(); - } - } - - // Call forward dynamics - bool isSucess = systemData.constraintSolver->SolveBoxedForwardDynamics( - engineOptions_->constraints.regularization, isStateUpToDate, ignoreBounds); - - /* Monitor number of successive constraint solving failure. Exception raising is - delegated to the 'step' method since this method is supposed to always succeed. */ - if (isSucess) - { - systemData.successiveSolveFailed = 0U; - } - else - { - if (engineOptions_->stepper.verbose) - { - std::cout << "Constraint solver failure." << std::endl; - } - ++systemData.successiveSolveFailed; - } - - // Restore contact frame forces and bounds internal efforts - systemData.constraints.foreach( - ConstraintNodeType::BOUNDS_JOINTS, - [&u = systemData.state.u, - &uInternal = systemData.state.uInternal, - &joints = const_cast(model.joints)]( - std::shared_ptr & constraint, - ConstraintNodeType /* node */) - { - if (!constraint->getIsEnabled()) - { - return; - } - - Eigen::VectorXd & uJoint = constraint->lambda_; - const auto & jointConstraint = - static_cast(*constraint.get()); - const auto & jointModel = joints[jointConstraint.getJointIndex()]; - jointModel.jointVelocitySelector(uInternal) += uJoint; - jointModel.jointVelocitySelector(u) += uJoint; - }); - - auto constraintIt = systemData.constraints.contactFrames.begin(); - auto forceIt = system.robot->contactForces_.begin(); - for (; constraintIt != systemData.constraints.contactFrames.end(); - ++constraintIt, ++forceIt) - { - auto & constraint = *constraintIt->second.get(); - if (!constraint.getIsEnabled()) - { - continue; - } - const auto & frameConstraint = static_cast(constraint); - - // Extract force in local reference-frame-aligned from lagrangian multipliers - pinocchio::Force fextInLocal(frameConstraint.lambda_.head<3>(), - frameConstraint.lambda_[3] * - Eigen::Vector3d::UnitZ()); - - // Compute force in local world aligned frame - const Eigen::Matrix3d & rotationLocal = frameConstraint.getLocalFrame(); - const pinocchio::Force fextInWorld({ - rotationLocal * fextInLocal.linear(), - rotationLocal * fextInLocal.angular(), - }); - - // Convert the force from local world aligned frame to local frame - const pinocchio::FrameIndex frameIndex = frameConstraint.getFrameIndex(); - const auto rotationWorldInContact = data.oMf[frameIndex].rotation().transpose(); - forceIt->linear().noalias() = rotationWorldInContact * fextInWorld.linear(); - forceIt->angular().noalias() = rotationWorldInContact * fextInWorld.angular(); - - // Convert the force from local world aligned to local parent joint - pinocchio::JointIndex parentJointIndex = model.frames[frameIndex].parent; - fext[parentJointIndex] += - convertForceGlobalFrameToJoint(model, data, frameIndex, fextInWorld); - } - - systemData.constraints.foreach( - ConstraintNodeType::COLLISION_BODIES, - [&fext, &model, &data](std::shared_ptr & constraint, - ConstraintNodeType /* node */) - { - if (!constraint->getIsEnabled()) - { - return; - } - const auto & frameConstraint = - static_cast(*constraint.get()); - - // Extract force in world frame from lagrangian multipliers - pinocchio::Force fextInLocal(frameConstraint.lambda_.head<3>(), - frameConstraint.lambda_[3] * - Eigen::Vector3d::UnitZ()); - - // Compute force in world frame - const Eigen::Matrix3d & rotationLocal = frameConstraint.getLocalFrame(); - const pinocchio::Force fextInWorld({ - rotationLocal * fextInLocal.linear(), - rotationLocal * fextInLocal.angular(), - }); - - // Convert the force from local world aligned to local parent joint - const pinocchio::FrameIndex frameIndex = frameConstraint.getFrameIndex(); - const pinocchio::JointIndex parentJointIndex = model.frames[frameIndex].parent; - fext[parentJointIndex] += - convertForceGlobalFrameToJoint(model, data, frameIndex, fextInWorld); - }); - - return data.ddq; - } - else - { - // No kinematic constraint: run aba algorithm - return pinocchio_overload::aba(model, data, q, v, u, fext); - } - } - - // =================================================================== - // ================ Log reading and writing utilities ================ - // =================================================================== - - std::shared_ptr EngineMultiRobot::getLog() - { - // Update internal log data buffer if uninitialized - if (!logData_) - { - logData_ = std::make_shared(telemetryRecorder_->getLog()); - } - - // Return shared pointer to internal log data buffer - return std::const_pointer_cast(logData_); - } - - LogData readLogHdf5(const std::string & filename) - { - LogData logData{}; - - // Open HDF5 logfile - std::unique_ptr file; - try - { - /* Specifying `H5F_CLOSE_STRONG` is necessary to completely close the file (including - all open objects) before returning. See: - https://docs.hdfgroup.org/hdf5/v1_12/group___f_a_p_l.html#ga60e3567f677fd3ade75b909b636d7b9c - */ - H5::FileAccPropList access_plist; - access_plist.setFcloseDegree(H5F_CLOSE_STRONG); - file = std::make_unique( - filename, H5F_ACC_RDONLY, H5::FileCreatPropList::DEFAULT, access_plist); - } - catch (const H5::FileIException &) - { - THROW_ERROR(std::runtime_error, - "Impossible to open the log file. Make sure it exists and " - "you have reading permissions."); - } - - // Extract all constants. There is no ordering among them, unlike variables. - H5::Group constantsGroup = file->openGroup("/constants"); - file->iterateElems( - "/constants", - NULL, - [](hid_t group, const char * name, void * op_data) -> herr_t - { - LogData * logDataPtr = static_cast(op_data); - H5::Group _constantsGroup(group); - const H5::DataSet constantDataSet = _constantsGroup.openDataSet(name); - const H5::DataSpace constantSpace = H5::DataSpace(H5S_SCALAR); - const H5::StrType constantDataType = constantDataSet.getStrType(); - const hssize_t numBytes = constantDataType.getSize(); - H5::StrType stringType(H5::PredType::C_S1, numBytes); - stringType.setStrpad(H5T_str_t::H5T_STR_NULLPAD); - std::string value(numBytes, '\0'); - constantDataSet.read(value.data(), stringType, constantSpace); - logDataPtr->constants.emplace_back(name, std::move(value)); - return 0; - }, - static_cast(&logData)); - - // Extract the times - const H5::DataSet globalTimeDataSet = file->openDataSet(std::string{GLOBAL_TIME}); - const H5::DataSpace timeSpace = globalTimeDataSet.getSpace(); - const hssize_t numData = timeSpace.getSimpleExtentNpoints(); - logData.times.resize(numData); - globalTimeDataSet.read(logData.times.data(), H5::PredType::NATIVE_INT64); - - // Add "unit" attribute to GLOBAL_TIME vector - const H5::Attribute unitAttrib = globalTimeDataSet.openAttribute("unit"); - unitAttrib.read(H5::PredType::NATIVE_DOUBLE, &logData.timeUnit); - - // Get the (partitioned) number of variables - H5::Group variablesGroup = file->openGroup("/variables"); - int64_t numInt = 0, numFloat = 0; - std::pair numVar{numInt, numFloat}; - H5Literate( - variablesGroup.getId(), - H5_INDEX_CRT_ORDER, - H5_ITER_INC, - NULL, - [](hid_t group, const char * name, const H5L_info_t * /* oinfo */, void * op_data) - -> herr_t - { - auto & [_numInt, _numFloat] = - *static_cast *>(op_data); - H5::Group fieldGroup = H5::Group(group).openGroup(name); - const H5::DataSet valueDataset = fieldGroup.openDataSet("value"); - const H5T_class_t valueType = valueDataset.getTypeClass(); - if (valueType == H5T_FLOAT) - { - ++_numFloat; - } - else - { - ++_numInt; - } - return 0; - }, - static_cast(&numVar)); - - // Pre-allocate memory - logData.integerValues.resize(numInt, numData); - logData.floatValues.resize(numFloat, numData); - VectorX intVector(numData); - VectorX floatVector(numData); - logData.variableNames.reserve(1 + numInt + numFloat); - logData.variableNames.emplace_back(GLOBAL_TIME); - - // Read all variables while preserving ordering - using opDataT = std::tuple &, VectorX &>; - opDataT opData{logData, intVector, floatVector}; - H5Literate( - variablesGroup.getId(), - H5_INDEX_CRT_ORDER, - H5_ITER_INC, - NULL, - [](hid_t group, const char * name, const H5L_info_t * /* oinfo */, void * op_data) - -> herr_t - { - auto & [logDataIn, intVectorIn, floatVectorIn] = *static_cast(op_data); - const Eigen::Index varIndex = logDataIn.variableNames.size() - 1; - const int64_t numIntIn = logDataIn.integerValues.rows(); - H5::Group fieldGroup = H5::Group(group).openGroup(name); - const H5::DataSet valueDataset = fieldGroup.openDataSet("value"); - if (varIndex < numIntIn) - { - valueDataset.read(intVectorIn.data(), H5::PredType::NATIVE_INT64); - logDataIn.integerValues.row(varIndex) = intVectorIn; - } - else - { - valueDataset.read(floatVectorIn.data(), H5::PredType::NATIVE_DOUBLE); - logDataIn.floatValues.row(varIndex - numIntIn) = floatVectorIn; - } - logDataIn.variableNames.push_back(name); - return 0; - }, - static_cast(&opData)); - - // Close file once done - file->close(); - - return logData; - } - - LogData EngineMultiRobot::readLog(const std::string & filename, const std::string & format) - { - if (format == "binary") - { - return TelemetryRecorder::readLog(filename); - } - if (format == "hdf5") - { - return readLogHdf5(filename); - } - THROW_ERROR(std::invalid_argument, - "Format '", - format, - "' not recognized. It must be either 'binary' or 'hdf5'."); - } - - void writeLogHdf5(const std::string & filename, const std::shared_ptr & logData) - { - // Open HDF5 logfile - std::unique_ptr file; - try - { - H5::FileAccPropList access_plist; - access_plist.setFcloseDegree(H5F_CLOSE_STRONG); - file = std::make_unique( - filename, H5F_ACC_TRUNC, H5::FileCreatPropList::DEFAULT, access_plist); - } - catch (const H5::FileIException & open_file) - { - THROW_ERROR(std::runtime_error, - "Impossible to create the log file. Make sure the root folder " - "exists and you have writing permissions."); - } - - // Add "VERSION" attribute - const H5::DataSpace versionSpace = H5::DataSpace(H5S_SCALAR); - const H5::Attribute versionAttrib = - file->createAttribute("VERSION", H5::PredType::NATIVE_INT32, versionSpace); - versionAttrib.write(H5::PredType::NATIVE_INT32, &logData->version); - - // Add "START_TIME" attribute - int64_t time = std::time(nullptr); - const H5::DataSpace startTimeSpace = H5::DataSpace(H5S_SCALAR); - const H5::Attribute startTimeAttrib = - file->createAttribute("START_TIME", H5::PredType::NATIVE_INT64, startTimeSpace); - startTimeAttrib.write(H5::PredType::NATIVE_INT64, &time); - - // Add GLOBAL_TIME vector - const hsize_t timeDims[1] = {hsize_t(logData->times.size())}; - const H5::DataSpace globalTimeSpace = H5::DataSpace(1, timeDims); - const H5::DataSet globalTimeDataSet = file->createDataSet( - std::string{GLOBAL_TIME}, H5::PredType::NATIVE_INT64, globalTimeSpace); - globalTimeDataSet.write(logData->times.data(), H5::PredType::NATIVE_INT64); - - // Add "unit" attribute to GLOBAL_TIME vector - const H5::DataSpace unitSpace = H5::DataSpace(H5S_SCALAR); - const H5::Attribute unitAttrib = - globalTimeDataSet.createAttribute("unit", H5::PredType::NATIVE_DOUBLE, unitSpace); - unitAttrib.write(H5::PredType::NATIVE_DOUBLE, &logData->timeUnit); - - // Add group "constants" - H5::Group constantsGroup(file->createGroup("constants")); - for (const auto & [key, value] : logData->constants) - { - // Define a dataset with a single string of fixed length - const H5::DataSpace constantSpace = H5::DataSpace(H5S_SCALAR); - H5::StrType stringType(H5::PredType::C_S1, std::max(value.size(), std::size_t(1))); - - // To tell parser continue reading if '\0' is encountered - stringType.setStrpad(H5T_str_t::H5T_STR_NULLPAD); - - // Write the constant - H5::DataSet constantDataSet = - constantsGroup.createDataSet(key, stringType, constantSpace); - constantDataSet.write(value, stringType); - } - - // Temporary contiguous storage for variables - VectorX intVector; - VectorX floatVector; - - // Get the number of integer and float variables - const Eigen::Index numInt = logData->integerValues.rows(); - const Eigen::Index numFloat = logData->floatValues.rows(); - - /* Add group "variables". - C++ helper `file->createGroup("variables")` cannot be used - because we want to preserve order. */ - hid_t group_creation_plist = H5Pcreate(H5P_GROUP_CREATE); - H5Pset_link_creation_order(group_creation_plist, - H5P_CRT_ORDER_TRACKED | H5P_CRT_ORDER_INDEXED); - hid_t group_id = H5Gcreate( - file->getId(), "/variables/", H5P_DEFAULT, group_creation_plist, H5P_DEFAULT); - H5::Group variablesGroup(group_id); - - // Store all integers - for (Eigen::Index i = 0; i < numInt; ++i) - { - const std::string & key = logData->variableNames[i]; - - // Create group for field - H5::Group fieldGroup = variablesGroup.createGroup(key); - - // Enable compression and shuffling - H5::DSetCreatPropList plist; - const hsize_t chunkSize[1] = {std::max(hsize_t(1), hsize_t(logData->times.size()))}; - plist.setChunk(1, chunkSize); // Read the whole vector at once. - plist.setShuffle(); - plist.setDeflate(4); - - // Create time dataset using symbolic link - fieldGroup.link(H5L_TYPE_HARD, toString("/", GLOBAL_TIME), "time"); - - // Create variable dataset - H5::DataSpace valueSpace = H5::DataSpace(1, timeDims); - H5::DataSet valueDataset = - fieldGroup.createDataSet("value", H5::PredType::NATIVE_INT64, valueSpace, plist); - - // Write values in one-shot for efficiency - intVector = logData->integerValues.row(i); - valueDataset.write(intVector.data(), H5::PredType::NATIVE_INT64); - } - - // Store all floats - for (Eigen::Index i = 0; i < numFloat; ++i) - { - const std::string & key = logData->variableNames[i + 1 + numInt]; - - // Create group for field - H5::Group fieldGroup(variablesGroup.createGroup(key)); - - // Enable compression and shuffling - H5::DSetCreatPropList plist; - const hsize_t chunkSize[1] = {std::max(hsize_t(1), hsize_t(logData->times.size()))}; - plist.setChunk(1, chunkSize); // Read the whole vector at once. - plist.setShuffle(); - plist.setDeflate(4); - - // Create time dataset using symbolic link - fieldGroup.link(H5L_TYPE_HARD, toString("/", GLOBAL_TIME), "time"); - - // Create variable dataset - H5::DataSpace valueSpace = H5::DataSpace(1, timeDims); - H5::DataSet valueDataset = - fieldGroup.createDataSet("value", H5::PredType::NATIVE_DOUBLE, valueSpace, plist); - - // Write values - floatVector = logData->floatValues.row(i); - valueDataset.write(floatVector.data(), H5::PredType::NATIVE_DOUBLE); - } - - // Close file once done - file->close(); - } - - void EngineMultiRobot::writeLog(const std::string & filename, const std::string & format) - { - // Make sure there is something to write - if (!isTelemetryConfigured_) - { - THROW_ERROR(bad_control_flow, - "Telemetry not configured. Please start a simulation before writing log."); - } - - // Pick the appropriate format - if (format == "binary") - { - telemetryRecorder_->writeLog(filename); - } - else if (format == "hdf5") - { - // Extract log data - std::shared_ptr logData = getLog(); - - // Write log data - writeLogHdf5(filename, logData); - } - else - { - THROW_ERROR(std::invalid_argument, - "Format '", - format, - "' not recognized. It must be either 'binary' or 'hdf5'."); - } - } -} diff --git a/core/src/engine/system.cc b/core/src/engine/system.cc deleted file mode 100644 index 13d881bee..000000000 --- a/core/src/engine/system.cc +++ /dev/null @@ -1,107 +0,0 @@ -#include "pinocchio/spatial/force.hpp" // `pinocchio::Force` -#include "pinocchio/algorithm/joint-configuration.hpp" // `pinocchio::neutral` - -#include "jiminy/core/robot/robot.h" -#include "jiminy/core/solver/constraint_solvers.h" -#include "jiminy/core/constraints/abstract_constraint.h" -#include "jiminy/core/control/abstract_controller.h" -#include "jiminy/core/engine/system.h" -#include "jiminy/core/utilities/helpers.h" - - -namespace jiminy -{ - // ******************************** External force functors ******************************** // - - ProfileForce::ProfileForce(const std::string & frameNameIn, - pinocchio::FrameIndex frameIndexIn, - double updatePeriodIn, - const ProfileForceFunction & forceFuncIn) noexcept : - frameName{frameNameIn}, - frameIndex{frameIndexIn}, - updatePeriod{updatePeriodIn}, - func{forceFuncIn} - { - } - - ImpulseForce::ImpulseForce(const std::string & frameNameIn, - pinocchio::FrameIndex frameIndexIn, - double tIn, - double dtIn, - const pinocchio::Force & forceIn) noexcept : - frameName{frameNameIn}, - frameIndex{frameIndexIn}, - t{tIn}, - dt{dtIn}, - force{forceIn} - { - } - - CouplingForce::CouplingForce(const std::string & systemName1In, - std::ptrdiff_t systemIndex1In, - const std::string & systemName2In, - std::ptrdiff_t systemIndex2In, - const std::string & frameName1In, - pinocchio::FrameIndex frameIndex1In, - const std::string & frameName2In, - pinocchio::FrameIndex frameIndex2In, - const CouplingForceFunction & forceFuncIn) noexcept : - systemName1{systemName1In}, - systemIndex1{systemIndex1In}, - systemName2{systemName2In}, - systemIndex2{systemIndex2In}, - frameName1{frameName1In}, - frameIndex1{frameIndex1In}, - frameName2{frameName2In}, - frameIndex2{frameIndex2In}, - func{forceFuncIn} - { - } - - // ************************************** System state ************************************* // - - void SystemState::initialize(const Robot & robot) - { - if (!robot.getIsInitialized()) - { - THROW_ERROR(bad_control_flow, "Robot not initialized."); - } - - Eigen::Index nv = robot.nv(); - std::size_t nMotors = robot.nmotors(); - std::size_t nJoints = robot.pinocchioModel_.njoints; - q = pinocchio::neutral(robot.pinocchioModel_); - v.setZero(nv); - a.setZero(nv); - command.setZero(nMotors); - u.setZero(nv); - uMotor.setZero(nMotors); - uInternal.setZero(nv); - uCustom.setZero(nv); - fExternal = ForceVector(nJoints, pinocchio::Force::Zero()); - isInitialized_ = true; - } - - bool SystemState::getIsInitialized() const - { - return isInitialized_; - } - - void SystemState::clear() - { - q.resize(0); - v.resize(0); - a.resize(0); - command.resize(0); - u.resize(0); - uMotor.resize(0); - uInternal.resize(0); - uCustom.resize(0); - fExternal.clear(); - } - - SystemData::SystemData() = default; - SystemData::SystemData(SystemData &&) = default; - SystemData & SystemData::operator=(SystemData &&) = default; - SystemData::~SystemData() = default; -} diff --git a/core/src/robot/robot.cc b/core/src/robot/robot.cc index 44244912d..c1d7f4fef 100644 --- a/core/src/robot/robot.cc +++ b/core/src/robot/robot.cc @@ -7,14 +7,16 @@ #include "jiminy/core/io/file_device.h" #include "jiminy/core/hardware/abstract_motor.h" #include "jiminy/core/hardware/abstract_sensor.h" +#include "jiminy/core/control/abstract_controller.h" +#include "jiminy/core/control/controller_functor.h" #include "jiminy/core/robot/robot.h" namespace jiminy { - - Robot::Robot() noexcept : + Robot::Robot(const std::string & name) noexcept : + name_{name}, motorSharedStorage_{std::make_shared()} { } @@ -26,6 +28,22 @@ namespace jiminy detachMotors(); } + std::shared_ptr MakeDefaultController(const std::shared_ptr & robot) + { + auto noop = [](double /* t */, + const Eigen::VectorXd & /* q */, + const Eigen::VectorXd & /* v */, + const SensorMeasurementTree & /* sensorMeasurements */, + Eigen::VectorXd & /* out */) + { + // Empty on purpose + }; + std::shared_ptr controller = + std::make_shared>(noop, noop); + controller->initialize(robot); + return controller; + } + void Robot::initialize(const std::string & urdfPath, bool hasFreeflyer, const std::vector & meshPackageDirs, @@ -37,7 +55,10 @@ namespace jiminy /* Delete the current model and generate a new one. Note that is also refresh all proxies automatically. */ - return Model::initialize(urdfPath, hasFreeflyer, meshPackageDirs, loadVisualMeshes); + Model::initialize(urdfPath, hasFreeflyer, meshPackageDirs, loadVisualMeshes); + + // Initialize default controller + controller_ = MakeDefaultController(shared_from_this()); } void Robot::initialize(const pinocchio::Model & pinocchioModel, @@ -48,23 +69,33 @@ namespace jiminy detachSensors(); detachMotors(); - /* Delete the current model and generate a new one. - Note that is also refresh all proxies automatically. */ - return Model::initialize(pinocchioModel, collisionModel, visualModel); + // Delete the current model and generate a new one + Model::initialize(pinocchioModel, collisionModel, visualModel); + + // Initialize default controller + controller_ = MakeDefaultController(shared_from_this()); + } + + const std::string & Robot::getName() const + { + return name_; } void Robot::reset(const uniform_random_bit_generator_ref & g) { - // Reset the model + // Reset telemetry flag + isTelemetryConfigured_ = false; + + // Reset model Model::reset(g); - // Reset the motors + // Reset motors if (!motors_.empty()) { (*motors_.begin())->resetAll(); } - // Reset the sensors + // Reset sensors for (auto & sensorGroupItem : sensors_) { if (!sensorGroupItem.second.empty()) @@ -73,31 +104,42 @@ namespace jiminy } } - // Reset the telemetry flag - isTelemetryConfigured_ = false; + // Reset controller + controller_->reset(); } - void Robot::configureTelemetry(std::shared_ptr telemetryData, - const std::string & prefix) + void Robot::configureTelemetry(std::shared_ptr telemetryData) { if (!isInitialized_) { THROW_ERROR(bad_control_flow, "Robot is initialized."); } - telemetryData_ = telemetryData; - isTelemetryConfigured_ = false; - for (const auto & [sensorType, sensorGroup] : sensors_) + telemetryData_ = telemetryData; + try { - for (const auto & sensor : sensorGroup) + // Configure hardware telemetry + for (const auto & [sensorType, sensorGroup] : sensors_) { - if (sensorTelemetryOptions_[sensorType]) + for (const auto & sensor : sensorGroup) { - sensor->configureTelemetry(telemetryData_, prefix); + if (telemetryOptions_[sensorType]) + { + sensor->configureTelemetry(telemetryData_, name_); + } } } + + // Configure controller telemetry + controller_->configureTelemetry(telemetryData_, name_); } + catch (...) + { + telemetryData_.reset(); + throw; + } + isTelemetryConfigured_ = true; } @@ -272,8 +314,7 @@ namespace jiminy if (sensorGroupIt == sensors_.end()) { sensorSharedStorageMap_.emplace(sensorType, std::make_shared()); - sensorTelemetryOptions_.emplace(sensorType, - true); // Enable the telemetry by default + telemetryOptions_.emplace(sensorType, true); // Enable telemetry by default } // Attach the sensor @@ -288,6 +329,11 @@ namespace jiminy void Robot::detachSensor(const std::string & sensorType, const std::string & sensorName) { + if (!isInitialized_) + { + THROW_ERROR(bad_control_flow, "Robot not initialized."); + } + if (getIsLocked()) { THROW_ERROR(std::logic_error, @@ -295,11 +341,6 @@ namespace jiminy "Please stop it before removing motors."); } - if (!isInitialized_) - { - THROW_ERROR(bad_control_flow, "Robot not initialized."); - } - // FIXME: remove explicit conversion to `std::string` when moving to C++20 auto sensorGroupIt = sensors_.find(std::string{sensorType}); if (sensorGroupIt == sensors_.end()) @@ -336,7 +377,7 @@ namespace jiminy { sensors_.erase(sensorType); sensorSharedStorageMap_.erase(sensorType); - sensorTelemetryOptions_.erase(sensorType); + telemetryOptions_.erase(sensorType); } // Refresh the sensors proxies @@ -378,6 +419,52 @@ namespace jiminy } } + void Robot::setController(const std::shared_ptr & controller) + { + // Make sure that the robot is not locked + if (getIsLocked()) + { + THROW_ERROR(std::logic_error, + "Robot already locked, probably because a simulation is running. " + "Please stop it before setting a new controller."); + } + + // Reset controller to default if none was specified + if (!controller) + { + controller_ = MakeDefaultController(shared_from_this()); + return; + } + + // Unbind the old controller to allow for initialization of the new controller + controller_.reset(); + + try + { + // Initialize the new controller for this robot + controller->initialize(shared_from_this()); + + // Set the controller + controller_ = controller; + } + catch (...) + { + // Reset controller to default before throwing exception in case of failure + setController({}); + throw; + } + } + + std::shared_ptr Robot::getController() + { + return controller_; + } + + std::weak_ptr Robot::getController() const + { + return std::const_pointer_cast(controller_); + } + void Robot::refreshProxies() { if (!isInitialized_) @@ -569,6 +656,7 @@ namespace jiminy void Robot::setOptions(const GenericConfig & robotOptions) { + // Set model options GenericConfig::const_iterator modelOptionsIt; modelOptionsIt = robotOptions.find("model"); if (modelOptionsIt == robotOptions.end()) @@ -579,6 +667,7 @@ namespace jiminy const GenericConfig & modelOptions = boost::get(modelOptionsIt->second); setModelOptions(modelOptions); + // Set motors options GenericConfig::const_iterator motorsOptionsIt; motorsOptionsIt = robotOptions.find("motors"); if (motorsOptionsIt == robotOptions.end()) @@ -589,6 +678,7 @@ namespace jiminy const GenericConfig & motorsOptions = boost::get(motorsOptionsIt->second); setMotorsOptions(motorsOptions); + // Set sensor options GenericConfig::const_iterator sensorOptionsIt = robotOptions.find("sensors"); if (sensorOptionsIt == robotOptions.end()) { @@ -598,6 +688,19 @@ namespace jiminy const GenericConfig & sensorOptions = boost::get(sensorOptionsIt->second); setSensorsOptions(sensorOptions); + // Set controller options + GenericConfig::const_iterator controllerOptionsIt; + controllerOptionsIt = robotOptions.find("controller"); + if (controllerOptionsIt == robotOptions.end()) + { + THROW_ERROR(std::invalid_argument, "'model' options are missing."); + } + + const GenericConfig & controllerOptions = + boost::get(controllerOptionsIt->second); + setControllerOptions(controllerOptions); + + // Set telemetry options GenericConfig::const_iterator telemetryOptionsIt = robotOptions.find("telemetry"); if (telemetryOptionsIt == robotOptions.end()) { @@ -617,32 +720,21 @@ namespace jiminy robotOptions["motors"] = getMotorsOptions(); GenericConfig sensorOptions; robotOptions["sensors"] = getSensorsOptions(); + GenericConfig controllerOptions; + robotOptions["controller"] = getControllerOptions(); GenericConfig telemetryOptions; robotOptions["telemetry"] = getTelemetryOptions(); return robotOptions; } - void Robot::setMotorOptions(const std::string & motorName, const GenericConfig & motorOptions) + void Robot::setModelOptions(const GenericConfig & modelOptions) { - if (getIsLocked()) - { - THROW_ERROR(std::logic_error, - "Robot already locked, probably because a simulation is running. " - "Please stop it before removing motors."); - } - - MotorVector::iterator motorIt; - motorIt = std::find_if(motors_.begin(), - motors_.end(), - [&motorName](const auto & elem) - { return (elem->getName() == motorName); }); - if (motorIt == motors_.end()) - { - THROW_ERROR( - std::invalid_argument, "None of the attached motors has name '", motorName, "'."); - } + return Model::setOptions(modelOptions); + } - (*motorIt)->setOptions(motorOptions); + GenericConfig Robot::getModelOptions() const + { + return Model::getOptions(); } void Robot::setMotorsOptions(const GenericConfig & motorsOptions) @@ -669,20 +761,6 @@ namespace jiminy } } - GenericConfig Robot::getMotorOptions(const std::string & motorName) const - { - auto motorIt = std::find_if(motors_.begin(), - motors_.end(), - [&motorName](const auto & elem) - { return (elem->getName() == motorName); }); - if (motorIt == motors_.end()) - { - THROW_ERROR( - std::invalid_argument, "None of the attached motors has name '", motorName, "'."); - } - return (*motorIt)->getOptions(); - } - GenericConfig Robot::getMotorsOptions() const { GenericConfig motorsOptions; @@ -693,78 +771,6 @@ namespace jiminy return motorsOptions; } - void Robot::setSensorOptions(const std::string & sensorType, - const std::string & sensorName, - const GenericConfig & sensorOptions) - { - if (getIsLocked()) - { - THROW_ERROR(std::logic_error, - "Robot already locked, probably because a simulation is running. " - "Please stop it before removing motors."); - } - - auto sensorGroupIt = sensors_.find(sensorType); - if (sensorGroupIt == sensors_.end()) - { - THROW_ERROR(std::invalid_argument, - "None of the attached sensors has type '", - sensorType, - "'."); - } - - auto sensorIt = std::find_if(sensorGroupIt->second.begin(), - sensorGroupIt->second.end(), - [&sensorName](const auto & elem) - { return (elem->getName() == sensorName); }); - if (sensorIt == sensorGroupIt->second.end()) - { - THROW_ERROR(std::invalid_argument, - "None of the attached sensors of type '", - sensorType, - "' has name '", - sensorName, - "'."); - } - - (*sensorIt)->setOptions(sensorOptions); - } - - void Robot::setSensorsOptions(const std::string & sensorType, - const GenericConfig & sensorsOptions) - { - if (getIsLocked()) - { - THROW_ERROR(std::logic_error, - "Robot already locked, probably because a simulation is running. " - "Please stop it before removing motors."); - } - - SensorTree::iterator sensorGroupIt; - sensorGroupIt = sensors_.find(sensorType); - if (sensorGroupIt == sensors_.end()) - { - THROW_ERROR(std::invalid_argument, - "None of the attached sensors has type '", - sensorType, - "'."); - } - - for (const auto & sensor : sensorGroupIt->second) - { - auto sensorOptionIt = sensorsOptions.find(sensor->getName()); - if (sensorOptionIt != sensorsOptions.end()) - { - sensor->setOptions(boost::get(sensorOptionIt->second)); - } - else - { - sensor->setOptionsAll(sensorsOptions); - break; - } - } - } - void Robot::setSensorsOptions(const GenericConfig & sensorsOptions) { if (getIsLocked()) @@ -813,54 +819,6 @@ namespace jiminy } } - GenericConfig Robot::getSensorOptions(const std::string & sensorType, - const std::string & sensorName) const - { - auto sensorGroupIt = sensors_.find(sensorType); - if (sensorGroupIt == sensors_.end()) - { - THROW_ERROR(std::invalid_argument, - "None of the attached sensors has type '", - sensorType, - "'."); - } - - auto sensorIt = std::find_if(sensorGroupIt->second.begin(), - sensorGroupIt->second.end(), - [&sensorName](const auto & elem) - { return (elem->getName() == sensorName); }); - if (sensorIt == sensorGroupIt->second.end()) - { - THROW_ERROR(std::invalid_argument, - "None of the attached sensors of type '", - sensorType, - "' has name '", - sensorName, - "'."); - } - - return (*sensorIt)->getOptions(); - } - - GenericConfig Robot::getSensorsOptions(const std::string & sensorType) const - { - auto sensorGroupIt = sensors_.find(sensorType); - if (sensorGroupIt == sensors_.end()) - { - THROW_ERROR(std::invalid_argument, - "None of the attached sensors has type '", - sensorType, - "'."); - } - - GenericConfig sensorsOptions{}; - for (const auto & sensor : sensorGroupIt->second) - { - sensorsOptions[sensor->getName()] = sensor->getOptions(); - } - return sensorsOptions; - } - GenericConfig Robot::getSensorsOptions() const { GenericConfig sensorsOptions; @@ -876,14 +834,14 @@ namespace jiminy return sensorsOptions; } - void Robot::setModelOptions(const GenericConfig & modelOptions) + void Robot::setControllerOptions(const GenericConfig & controllerOptions) { - return Model::setOptions(modelOptions); + return controller_->setOptions(controllerOptions); } - GenericConfig Robot::getModelOptions() const + GenericConfig Robot::getControllerOptions() const { - return Model::getOptions(); + return controller_->getOptions(); } void Robot::setTelemetryOptions(const GenericConfig & telemetryOptions) @@ -895,7 +853,7 @@ namespace jiminy "Please stop it before removing motors."); } - for (auto & [sensorType, sensorGroupTelemetryOption] : sensorTelemetryOptions_) + for (auto & [sensorType, sensorGroupTelemetryOption] : telemetryOptions_) { const std::string optionTelemetryName = toString("enable", sensorType, "s"); auto sensorTelemetryOptionIt = telemetryOptions.find(optionTelemetryName); @@ -910,7 +868,7 @@ namespace jiminy GenericConfig Robot::getTelemetryOptions() const { GenericConfig telemetryOptions; - for (const auto & [sensorType, sensorGroupTelemetryOption] : sensorTelemetryOptions_) + for (const auto & [sensorType, sensorGroupTelemetryOption] : telemetryOptions_) { const std::string optionTelemetryName = toString("enable", sensorType, "s"); telemetryOptions[optionTelemetryName] = sensorGroupTelemetryOption; @@ -1045,6 +1003,7 @@ namespace jiminy void Robot::updateTelemetry() { + // Update hardware telemetry for (const auto & sensorGroupItem : sensors_) { if (!sensorGroupItem.second.empty()) @@ -1052,6 +1011,9 @@ namespace jiminy (*sensorGroupItem.second.begin())->updateTelemetryAll(); } } + + // Update controller telemetry + controller_->updateTelemetry(); } std::unique_ptr Robot::getLock() diff --git a/core/src/solver/constraint_solvers.cc b/core/src/solver/constraint_solvers.cc index 3fafbc042..c330bfbf1 100644 --- a/core/src/solver/constraint_solvers.cc +++ b/core/src/solver/constraint_solvers.cc @@ -12,7 +12,13 @@ namespace jiminy { - inline constexpr double PGS_MIN_REGULARIZER{1.0e-11}; + inline constexpr double MIN_REGULARIZER{1.0e-11}; + + inline constexpr double RELAX_MIN{0.01}; + inline constexpr double RELAX_MAX{1.0}; + inline constexpr uint32_t RELAX_MIN_ITER_NUM{20}; + inline constexpr uint32_t RELAX_MAX_ITER_NUM{30}; + inline constexpr double RELAX_SLOPE_ORDER{2.0}; PGSSolver::PGSSolver(const pinocchio::Model * model, pinocchio::Data * data, @@ -99,6 +105,7 @@ namespace jiminy void PGSSolver::ProjectedGaussSeidelIter(const Eigen::MatrixXd & A, const Eigen::VectorXd::SegmentReturnType & b, + const double w, Eigen::VectorXd::SegmentReturnType & x) { // First, loop over all unbounded constraints @@ -121,8 +128,8 @@ namespace jiminy } /* Second, loop over all bounds constraints. - Update breadth-first to converge faster. The deeper is it the more coefficients at the - very end. Note that the maximum number of blocks per constraint is 3. */ + Update breadth-first for faster convergence, knowing that the number of blocks + per constraint cannot exceed 3. */ for (std::size_t i = 0; i < 3; ++i) { for (const ConstraintData & constraintData : constraintsData_) @@ -168,11 +175,11 @@ namespace jiminy A_max = A_kk; } } - e += y_[i0] / A_max; + e += w * y_[i0] / A_max; for (std::uint_fast8_t j = 1; j < fSize - 1; ++j) { const Eigen::Index k = o + fIndex[j]; - x[k] += y_[k] / A_max; + x[k] += w * y_[k] / A_max; } // Project the coefficient between lower and upper bounds @@ -221,7 +228,10 @@ namespace jiminy tolerance, even if unconstrained. It seems to be related to compounding of errors, maybe due to the recursive computations. */ - assert(b.size() > 0 && "The number of inequality constraints must be larger than 0."); + if (b.size() == 0) + { + throw std::logic_error("The number of inequality constraints must be larger than 0."); + } // Reset the residuals y_.setZero(); @@ -232,15 +242,82 @@ namespace jiminy // Backup previous residuals yPrev_ = y_; - // Do a single iteration - ProjectedGaussSeidelIter(A, b, x); + // Update the under-relaxation factor + const double ratio = (static_cast(iterMax_ - RELAX_MIN_ITER_NUM) - iter) / + (iterMax_ - RELAX_MIN_ITER_NUM - RELAX_MAX_ITER_NUM); + double w = RELAX_MAX; + if (ratio < 1.0) + { + w = RELAX_MIN; + if (ratio > 0.0) + { + w += (RELAX_MAX - RELAX_MIN) * std::pow(ratio, RELAX_SLOPE_ORDER); + } + } - // Check if terminate conditions are satisfied - const double tol = tolAbs_ + tolRel_ * y_.cwiseAbs().maxCoeff(); + // Do one iteration + ProjectedGaussSeidelIter(A, b, w, x); + + /* Abort if stopping criteria is met. + It is not possible to define a stopping criteria on the residuals directly because + they can grow arbitrary large for constraints whose bounds are active. It follows + that stagnation of residuals is the only viable criteria. + The PGS algorithm has been modified for solving second-order cone LCP, which means + that only the L2-norm of the tangential forces can be expected to converge. Because + of this, it is too restrictive to check the element-wise variation of the residuals + over iterations. It makes more sense to look at the Linf-norm instead, but this + criteria is very lax. A good compromise may be to look at the constraint-block-wise + L2-norm, which is similar to what Drake simulator is doing. For reference, see: + https://github.com/RobotLocomotion/drake/blob/master/multibody/contact_solvers/pgs_solver.cc + */ + const double tol = tolAbs_ + tolRel_ * y_.lpNorm() + EPS; if (((y_ - yPrev_).array().abs() < tol).all()) { return true; } + + // std::cout << "[" << iter << "] (" << w << "): "; + // bool isSuccess = true; + // for (std::size_t i = 0; i < 3; ++i) + //{ + // for (const ConstraintData & constraintData : constraintsData_) + // { + // if (constraintData.isInactive || constraintData.nBlocks <= i) + // { + // continue; + // } + // + // const ConstraintBlock & block = constraintData.blocks[i]; + // const Eigen::Index * fIndex = block.fIndex; + // const Eigen::Index i0 = constraintData.startIndex + fIndex[0]; + // double yNorm = y_[i0] * y_[i0]; + // double yPrevNorm = yPrev_[i0] * yPrev_[i0]; + // for (std::uint_fast8_t j = 1; j < block.fSize - 1; ++j) + // { + // yNorm += y_[fIndex[j]] * y_[fIndex[j]]; + // yPrevNorm += yPrev_[fIndex[j]] * yPrev_[fIndex[j]]; + // } + // yNorm = std::sqrt(yNorm); + // yPrevNorm = std::sqrt(yPrevNorm); + // + // const double tol = tolAbs_ + tolRel_ * yNorm + EPS; + // std::cout << std::abs(yNorm - yPrevNorm) << "(" << tol << "), "; + // if (std::abs(yNorm - yPrevNorm) > tol) + // { + // isSuccess = false; + // break; + // } + // } + // if (!isSuccess) + // { + // break; + // } + // } + // std::cout << std::endl; + // if (isSuccess) + //{ + // return true; + // } } // Impossible to converge @@ -297,7 +374,7 @@ namespace jiminy See: - http://mujoco.org/book/modeling.html#CSolver - http://mujoco.org/book/computation.html#soParameters */ - A.diagonal() += (A.diagonal() * dampingInv).cwiseMax(PGS_MIN_REGULARIZER); + A.diagonal() += (A.diagonal() * dampingInv).cwiseMax(MIN_REGULARIZER); /* The LCP is not fully up-to-date since the upper triangular part is still missing. This will only be done if necessary. */ diff --git a/core/src/stepper/abstract_runge_kutta_stepper.cc b/core/src/stepper/abstract_runge_kutta_stepper.cc index a16a461ad..ca7266ffe 100644 --- a/core/src/stepper/abstract_runge_kutta_stepper.cc +++ b/core/src/stepper/abstract_runge_kutta_stepper.cc @@ -38,7 +38,7 @@ namespace jiminy stateIncrement_.setZero(); for (Eigen::Index j = 0; j < i; ++j) { - /* Equivalent to `stateIncrement_ += dt * A_(i, j) * ki_[j]` + /* Equivalent to `stateIncrement_ += (dt * A_(i, j)) * ki_[j]` but more efficient because it avoid temporaries. */ stateIncrement_.sumInPlace(ki_[j], dt * A_(i, j)); } diff --git a/core/src/stepper/runge_kutta_dopri_stepper.cc b/core/src/stepper/runge_kutta_dopri_stepper.cc index 827e1efa5..5b1e4dcd9 100644 --- a/core/src/stepper/runge_kutta_dopri_stepper.cc +++ b/core/src/stepper/runge_kutta_dopri_stepper.cc @@ -31,7 +31,7 @@ namespace jiminy scale_.absInPlace(); scale_ *= tolRel_; scale_ += tolAbs_; - + // Compute alternative solution stateIncrement_.setZero(); for (std::size_t i = 0; i < ki_.size(); ++i) @@ -70,8 +70,7 @@ namespace jiminy { /* Prevent numeric rounding error when close to zero. Multiply step size by 'DOPRI::SAFETY / (error ** (1 / DOPRI::STEPPER_ORDER))', - up to 'DOPRI::MAX_FACTOR'. - */ + up to 'DOPRI::MAX_FACTOR'. */ const double clippedError = std::max( error, std::pow(DOPRI::MAX_FACTOR / DOPRI::SAFETY, -DOPRI::STEPPER_ORDER)); dt *= DOPRI::SAFETY * std::pow(clippedError, -1.0 / DOPRI::STEPPER_ORDER); diff --git a/core/src/utilities/geometry.cc b/core/src/utilities/geometry.cc index 3e69655ea..0f87c8dd2 100644 --- a/core/src/utilities/geometry.cc +++ b/core/src/utilities/geometry.cc @@ -70,7 +70,7 @@ namespace jiminy // clang-format on } - /// @brief Compute the minor defined as the determinant of the sub-matrix formed by + /// \brief Compute the minor defined as the determinant of the sub-matrix formed by /// deleting row i and col j template double cofactor() const @@ -131,14 +131,14 @@ namespace jiminy #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wmaybe-uninitialized" - /// @brief Fast mesh simplification utility. + /// \brief Fast mesh simplification utility. /// - /// @details The original algorithm has been developed by Michael Garland and Paul + /// \details The original algorithm has been developed by Michael Garland and Paul /// Heckbert. The technical details can be found in their paper, "Surface /// Simplification Using Quadric Error Metrics.", 1997: /// http://www.cs.cmu.edu/~garland/Papers/quadrics.pdf /// - /// @sa The proposed implementation is based on code from Sven Forstmann released in 2014 + /// \sa The proposed implementation is based on code from Sven Forstmann released in 2014 /// under the MIT License: /// https://github.com/sp4cerat/Fast-Quadric-Mesh-Simplification class MeshSimplifier @@ -164,7 +164,7 @@ namespace jiminy } } - /// @brief Compute error for one specific edge. + /// \brief Compute error for one specific edge. std::pair computeError(std::size_t id_v1, std::size_t id_v2) { // Extract the relevant vertices @@ -221,7 +221,7 @@ namespace jiminy return {err1, p1}; } - /// @brief Check if a triangle flips when this edge is removed. + /// \brief Check if a triangle flips when this edge is removed. bool flipped(const Eigen::Vector3d & p, std::size_t i1, const Vertex & v0, @@ -253,7 +253,7 @@ namespace jiminy return false; } - /// @brief Update triangle connections and edge error after a edge is collapsed. + /// \brief Update triangle connections and edge error after a edge is collapsed. void collapseTriangles(const std::size_t & i0, const Vertex & v, std::vector & is_deleted, @@ -419,7 +419,7 @@ namespace jiminy } } - /// @brief Main simplification function. + /// \brief Main simplification function. void simplify(std::size_t mesh_update_rate = 5, double aggressiveness = 7, int64_t max_iter = 100, diff --git a/core/src/utilities/random.cc b/core/src/utilities/random.cc index 145bc45fe..48b8cd62a 100644 --- a/core/src/utilities/random.cc +++ b/core/src/utilities/random.cc @@ -176,10 +176,10 @@ namespace jiminy return (x << r) | (x >> (32 - r)); } - /// @brief MurmurHash3 is a non-cryptographic hash function initially designed + /// \brief MurmurHash3 is a non-cryptographic hash function initially designed /// for hash-based lookup. /// - /// @sa It was written by Austin Appleby, and is placed in the public domain. + /// \sa It was written by Austin Appleby, and is placed in the public domain. /// The author hereby disclaims copyright to this source code: /// https://github.com/aappleby/smhasher/blob/master/src/MurmurHash3.cpp static uint32_t MurmurHash3(const void * key, int32_t len, uint32_t seed) noexcept diff --git a/core/unit/engine_sanity_check.cc b/core/unit/engine_sanity_check.cc index af7dc1343..4de859b50 100755 --- a/core/unit/engine_sanity_check.cc +++ b/core/unit/engine_sanity_check.cc @@ -41,11 +41,6 @@ void internalDynamics(double /* t */, { } -bool callback(double /* t */, const Eigen::VectorXd & /* q */, const Eigen::VectorXd & /* v */) -{ - return true; -} - TEST(EngineSanity, EnergyConservation) { @@ -82,12 +77,13 @@ TEST(EngineSanity, EnergyConservation) } robot->setMotorsOptions(motorsOptions); - auto controller = std::make_shared>(computeCommand, internalDynamics); - controller->initialize(robot); + // Instantiate the controller + robot->setController( + std::make_shared>(computeCommand, internalDynamics)); // Create engine Engine engine{}; - engine.initialize(robot, controller, callback); + engine.addRobot(robot); // Configure engine: High accuracy + Continuous-time integration GenericConfig simuOptions = engine.getDefaultEngineOptions(); diff --git a/data/bipedal_robots/atlas/atlas_v4_options.toml b/data/bipedal_robots/atlas/atlas_v4_options.toml index 63d5a9b6f..f86fea921 100644 --- a/data/bipedal_robots/atlas/atlas_v4_options.toml +++ b/data/bipedal_robots/atlas/atlas_v4_options.toml @@ -3,6 +3,8 @@ [engine.stepper] verbose = false odeSolver = "euler_explicit" +tolAbs = 1e-5 +tolRel = 1e-4 sensorsUpdatePeriod = 0.005 controllerUpdatePeriod = 0.005 logInternalStepperSteps = false @@ -19,3 +21,10 @@ model = "constraint" stabilizationFreq = 25.0 transitionEps = 5.0e-3 friction = 0.8 +torsion = 0.0 + +# ======== Joints bounds configuration ======== + +[robot.model.joints] +enablePositionLimit = true +enableVelocityLimit = true diff --git a/data/bipedal_robots/cassie/cassie_options.toml b/data/bipedal_robots/cassie/cassie_options.toml index 4ed0ded67..9932dc2f3 100644 --- a/data/bipedal_robots/cassie/cassie_options.toml +++ b/data/bipedal_robots/cassie/cassie_options.toml @@ -3,6 +3,8 @@ [engine.stepper] verbose = false odeSolver = "euler_explicit" +tolAbs = 1e-5 +tolRel = 1e-4 sensorsUpdatePeriod = 0.005 controllerUpdatePeriod = 0.005 logInternalStepperSteps = false @@ -19,3 +21,9 @@ model = "constraint" stabilizationFreq = 25.0 transitionEps = 2.0e-3 friction = 0.5 + +# ======== Joints bounds configuration ======== + +[robot.model.joints] +enablePositionLimit = true +enableVelocityLimit = true diff --git a/data/bipedal_robots/digit/digit_options.toml b/data/bipedal_robots/digit/digit_options.toml index 6c1a540fa..9932dc2f3 100644 --- a/data/bipedal_robots/digit/digit_options.toml +++ b/data/bipedal_robots/digit/digit_options.toml @@ -3,12 +3,14 @@ [engine.stepper] verbose = false odeSolver = "euler_explicit" +tolAbs = 1e-5 +tolRel = 1e-4 sensorsUpdatePeriod = 0.005 controllerUpdatePeriod = 0.005 logInternalStepperSteps = false randomSeedSeq = [0,] -# ============== Ground dynamics =============== +# ============== Contact dynamics =============== [engine.constraints] solver = "PGS" @@ -22,6 +24,6 @@ friction = 0.5 # ======== Joints bounds configuration ======== -[system.robot.model.joints] +[robot.model.joints] enablePositionLimit = true enableVelocityLimit = true diff --git a/data/quadrupedal_robots/anymal/anymal_options.toml b/data/quadrupedal_robots/anymal/anymal_options.toml index e16cb97f8..6269f5675 100644 --- a/data/quadrupedal_robots/anymal/anymal_options.toml +++ b/data/quadrupedal_robots/anymal/anymal_options.toml @@ -3,12 +3,14 @@ [engine.stepper] verbose = false odeSolver = "euler_explicit" +tolAbs = 1e-5 +tolRel = 1e-4 sensorsUpdatePeriod = 0.005 controllerUpdatePeriod = 0.005 logInternalStepperSteps = false randomSeedSeq = [0,] -# ============== Ground dynamics =============== +# ============== Contact dynamics =============== [engine.constraints] solver = "PGS" @@ -22,6 +24,6 @@ friction = 1.0 # ======== Joints bounds configuration ======== -[system.robot.model.joints] +[robot.model.joints] enablePositionLimit = true enableVelocityLimit = true diff --git a/data/toys_models/ant/ant_options.toml b/data/toys_models/ant/ant_options.toml index 0051ad794..0a3debfef 100644 --- a/data/toys_models/ant/ant_options.toml +++ b/data/toys_models/ant/ant_options.toml @@ -22,6 +22,6 @@ friction = 1.0 # ======== Joints bounds configuration ======== -[system.robot.model.joints] +[robot.model.joints] enablePositionLimit = true enableVelocityLimit = false diff --git a/docs/api/jiminy/engine.rst b/docs/api/jiminy/engine.rst index aee62fd40..b0cfade08 100644 --- a/docs/api/jiminy/engine.rst +++ b/docs/api/jiminy/engine.rst @@ -11,12 +11,7 @@ engine :members: :undoc-members: -.. doxygenstruct:: jiminy::SystemState - :project: jiminy - :members: - :undoc-members: - -.. doxygenclass:: jiminy::EngineMultiRobot +.. doxygenstruct:: jiminy::RobotState :project: jiminy :members: :undoc-members: diff --git a/docs/api/jiminy_py/core/engine.rst b/docs/api/jiminy_py/core/engine.rst index d681eb5a4..49b469b57 100644 --- a/docs/api/jiminy_py/core/engine.rst +++ b/docs/api/jiminy_py/core/engine.rst @@ -1,12 +1,7 @@ engine ====== -.. autoclass:: jiminy_py.core.system - :members: - :undoc-members: - :show-inheritance: - -.. autoclass:: jiminy_py.core.EngineMultiRobot +.. autoclass:: jiminy_py.core.Engine :members: :undoc-members: :show-inheritance: @@ -16,12 +11,7 @@ engine :undoc-members: :show-inheritance: -.. autoclass:: jiminy_py.core.SystemState - :members: - :undoc-members: - :show-inheritance: - -.. autoclass:: jiminy_py.core.Engine +.. autoclass:: jiminy_py.core.RobotState :members: :undoc-members: :show-inheritance: diff --git a/python/gym_jiminy/common/gym_jiminy/common/bases/generic_bases.py b/python/gym_jiminy/common/gym_jiminy/common/bases/generic_bases.py index d9a9e20bf..0d25fd927 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/bases/generic_bases.py +++ b/python/gym_jiminy/common/gym_jiminy/common/bases/generic_bases.py @@ -179,7 +179,7 @@ class InterfaceJiminyEnv( simulator: Simulator robot: jiminy.Robot stepper_state: jiminy.StepperState - system_state: jiminy.SystemState + robot_state: jiminy.RobotState sensor_measurements: SensorMeasurementStackMap is_simulation_running: npt.NDArray[np.bool_] diff --git a/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline_bases.py b/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline_bases.py index 95a54b7a4..41b05b206 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline_bases.py +++ b/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline_bases.py @@ -77,7 +77,7 @@ def __init__(self, self.simulator = env.simulator self.robot = env.robot self.stepper_state = env.stepper_state - self.system_state = env.system_state + self.robot_state = env.robot_state self.sensor_measurements = env.sensor_measurements self.is_simulation_running = env.is_simulation_running @@ -267,7 +267,7 @@ def _setup(self) -> None: # Refresh some proxies for fast lookup self.robot = self.env.robot - self.system_state = self.env.system_state + self.robot_state = self.env.robot_state self.sensor_measurements = self.env.sensor_measurements # Initialize specialized operator(s) for efficiency diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py b/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py index 320b72986..910dbec75 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py @@ -60,6 +60,12 @@ from .internal import loop_interactive +# Maximum realtime slowdown of simulation steps before triggering timeout error +TIMEOUT_RATIO = 50 + +# Absolute tolerance when checking that observations are valid +OBS_CONTAINS_TOL = 0.01 + # Define universal bounds for the observation space FREEFLYER_POS_TRANS_MAX = 1000.0 FREEFLYER_VEL_LIN_MAX = 1000.0 @@ -73,8 +79,6 @@ SENSOR_GYRO_MAX = 100.0 SENSOR_ACCEL_MAX = 10000.0 -OBS_CONTAINS_TOL = 0.01 - LOGGER = logging.getLogger(__name__) @@ -181,13 +185,13 @@ def __init__(self, # Define some proxies for fast access self.engine: jiminy.Engine = self.simulator.engine - self.robot = self.engine.robot + self.robot = self.simulator.robot self.stepper_state = self.simulator.stepper_state self.is_simulation_running = self.simulator.is_simulation_running - self.system_state = self.engine.system_state - self._system_state_q = self.system_state.q - self._system_state_v = self.system_state.v - self._system_state_a = self.system_state.a + self.robot_state = self.simulator.robot_state + self._robot_state_q = self.robot_state.q + self._robot_state_v = self.robot_state.v + self._robot_state_a = self.robot_state.a self.sensor_measurements: SensorMeasurementStackMap = OrderedDict( self.robot.sensor_measurements) @@ -677,7 +681,8 @@ def reset(self, # type: ignore[override] # Remove external forces, if any self.simulator.remove_all_forces() - # Make sure the environment is properly setup + # Make sure the environment is properly setup. + # Note that the robot is not allowed to change after this point. self._setup() # Make sure the low-level engine has not changed, @@ -689,7 +694,7 @@ def reset(self, # type: ignore[override] # Re-initialize some shared memories. # It is necessary because the robot may have changed. - self.system_state = self.engine.system_state + self.robot_state = self.simulator.robot_state self.sensor_measurements = OrderedDict(self.robot.sensor_measurements) # Enforce the low-level controller. @@ -699,9 +704,7 @@ def reset(self, # type: ignore[override] # re-initialize the existing one by calling `controller.initialize` # method BEFORE calling `reset` method because doing otherwise would # cause a segfault. - noop_controller = jiminy.FunctionalController() - noop_controller.initialize(self.robot) - self.simulator.set_controller(noop_controller) + self.robot.controller = None # Reset the simulator. # Do NOT remove all forces since it has already been done before, and @@ -742,17 +745,15 @@ def reset(self, # type: ignore[override] # Instantiate the actual controller. # Note that a weak reference must be used to avoid circular reference. - controller = jiminy.FunctionalController( + self.robot.controller = jiminy.FunctionalController( partial(type(env)._controller_handle, weakref.proxy(env))) - controller.initialize(self.robot) - self.simulator.set_controller(controller) # Configure the maximum number of steps self.max_steps = int(self.simulation_duration_max // self.step_dt) # Register user-specified variables to the telemetry for header, value in self._registered_variables.values(): - register_variables(controller, header, value) + register_variables(self.robot.controller, header, value) # Sample the initial state and reset the low-level engine qpos, qvel = self._sample_state() @@ -767,11 +768,11 @@ def reset(self, # type: ignore[override] self.simulator.start( qpos, qvel, None, self.simulator.use_theoretical_model) - # Refresh system_state proxies. It must be done here because memory is + # Refresh robot_state proxies. It must be done here because memory is # only allocated by the engine when starting a simulation. - self._system_state_q = self.system_state.q - self._system_state_v = self.system_state.v - self._system_state_a = self.system_state.a + self._robot_state_q = self.robot_state.q + self._robot_state_v = self.robot_state.v + self._robot_state_a = self.robot_state.a # Initialize shared buffers self._initialize_buffers() @@ -782,8 +783,8 @@ def reset(self, # type: ignore[override] # Initialize the observation env._observer_handle( self.stepper_state.t, - self._system_state_q, - self._system_state_v, + self._robot_state_q, + self._robot_state_v, self.robot.sensor_measurements) # Initialize specialized most-derived observation clipping operator @@ -898,12 +899,12 @@ def step(self, # type: ignore[override] # of the every integration steps, during the controller update. self._env_derived._observer_handle( self.stepper_state.t, - self._system_state_q, - self._system_state_v, + self._robot_state_q, + self._robot_state_v, self.robot.sensor_measurements) # Make sure there is no 'nan' value in observation - if is_nan(self._system_state_a): + if is_nan(self._robot_state_a): raise RuntimeError( "The acceleration of the system is 'nan'. Something went " "wrong with jiminy engine.") @@ -1141,7 +1142,7 @@ def _interact(key: Optional[str] = None) -> bool: obs = self.observation self.render() if not enable_is_done and self.robot.has_freeflyer: - return self._system_state_q[2] < 0.0 + return self._robot_state_q[2] < 0.0 return terminated or truncated # Run interactive loop @@ -1285,11 +1286,12 @@ def _setup(self) -> None: # Configure the low-level integrator engine_options = self.simulator.engine.get_options() engine_options["stepper"]["iterMax"] = 0 - engine_options["stepper"]["dtMax"] = min(0.02, self.step_dt) - engine_options["stepper"]["logInternalStepperSteps"] = False + if self.debug: + engine_options["stepper"]["verbose"] = True + engine_options["stepper"]["logInternalStepperSteps"] = True # Set maximum computation time for single internal integration steps - engine_options["stepper"]["timeout"] = 2.0 + engine_options["stepper"]["timeout"] = self.step_dt * TIMEOUT_RATIO if self.debug: engine_options["stepper"]["timeout"] = 0.0 diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py b/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py index 338b5d124..9644f2105 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py @@ -1,7 +1,7 @@ """Generic environment to learn locomotion skills for legged robots using Jiminy simulator as physics engine. """ -from typing import Optional, Dict, Union, Callable, Any, Type, Sequence, Tuple +from typing import Optional, Dict, Union, Any, Type, Sequence, Tuple import numpy as np @@ -62,11 +62,6 @@ DEFAULT_HLC_TO_LLC_RATIO = 1 # (NA) -ImpulseForceType = Dict[str, Union[str, float, np.ndarray]] -ProfileForceFunc = Callable[[float, np.ndarray, np.ndarray, np.ndarray], None] -ProfileForceType = Dict[str, Union[str, ProfileForceFunc]] - - class WalkerJiminyEnv(BaseJiminyEnv): """Gym environment for learning locomotion skills for legged robots. @@ -367,7 +362,7 @@ def has_terminated(self) -> Tuple[bool, bool]: terminated, truncated = super().has_terminated() # Check if the agent has successfully solved the task - if self._system_state_q[2] < self._height_neutral * 0.5: + if self._robot_state_q[2] < self._height_neutral * 0.5: terminated = True return terminated, truncated @@ -397,7 +392,7 @@ def compute_reward(self, if 'energy' in reward_mixture_keys: v_mot = self.robot.sensor_measurements[encoder.type][1] - command = self.system_state.command + command = self.robot_state.command power_consumption = np.sum(np.maximum(command * v_mot, 0.0)) power_consumption_rel = \ power_consumption / self._power_consumption_max diff --git a/python/gym_jiminy/envs/gym_jiminy/envs/ant.py b/python/gym_jiminy/envs/gym_jiminy/envs/ant.py index 8ffa0ba40..6c10161f6 100644 --- a/python/gym_jiminy/envs/gym_jiminy/envs/ant.py +++ b/python/gym_jiminy/envs/gym_jiminy/envs/ant.py @@ -151,9 +151,9 @@ def refresh_observation(self, measurement: EngineObsType) -> None: if not self.is_simulation_running: # Initialize observation chunks self.obs_chunks = [ - self._system_state_q[2:], - self._system_state_v, - *[f.vector for f in self.system_state.f_external] + self._robot_state_q[2:], + self._robot_state_v, + *[f.vector for f in self.robot_state.f_external] ] # Initialize observation chunks sizes @@ -165,7 +165,7 @@ def refresh_observation(self, measurement: EngineObsType) -> None: idx_start = idx_end # Initialize previous torso position - self.xpos_prev = self._system_state_q[0] + self.xpos_prev = self._robot_state_q[0] # Update observation buffer assert isinstance(self.observation_space, gym.spaces.Box) @@ -177,7 +177,7 @@ def refresh_observation(self, measurement: EngineObsType) -> None: # Transform observed linear velocity to be in world frame self.observation[slice(*self.obs_chunks_sizes[1])][:3] = \ - Quaternion(self._system_state_q[3:7]) * self.obs_chunks[1][:3] + Quaternion(self._robot_state_q[3:7]) * self.obs_chunks[1][:3] def has_terminated(self) -> Tuple[bool, bool]: """ TODO: Write documentation. @@ -186,7 +186,7 @@ def has_terminated(self) -> Tuple[bool, bool]: terminated, truncated = super().has_terminated() # Check if the agent is jumping far too high or stuck on its back - zpos = self._system_state_q[2] + zpos = self._robot_state_q[2] if 1.0 < zpos or zpos < 0.2: truncated = True @@ -202,7 +202,7 @@ def compute_reward(self, reward = 0.0 # Compute forward velocity reward - xpos = self._system_state_q[0] + xpos = self._robot_state_q[0] forward_reward = (xpos - self.xpos_prev) / self.step_dt ctrl_cost = 0.5 * np.square(self.action).sum() diff --git a/python/gym_jiminy/unit_py/test_pipeline_control.py b/python/gym_jiminy/unit_py/test_pipeline_control.py index 16ed2d87d..72d177634 100644 --- a/python/gym_jiminy/unit_py/test_pipeline_control.py +++ b/python/gym_jiminy/unit_py/test_pipeline_control.py @@ -54,7 +54,8 @@ def _test_pid_standing(self): # Check that the final posture matches the expected one data_dir = os.path.join(os.path.dirname(__file__), "data") - img_prefix = '_'.join((self.env.robot.name, "standing", "*")) + model_name = self.env.robot.pinocchio_model.name + img_prefix = '_'.join((model_name, "standing", "*")) img_min_diff = np.inf for img_fullpath in glob(os.path.join(data_dir, img_prefix)): rgba_array_rel_ref = plt.imread(img_fullpath) @@ -75,7 +76,7 @@ def _test_pid_standing(self): raw_bytes = io.BytesIO() img_obj.save(raw_bytes, "PNG") raw_bytes.seek(0) - print(f"{self.env.robot.name} - {self.env.viewer.backend}:", + print(f"{model_name} - {self.env.viewer.backend}:", base64.b64encode(raw_bytes.read())) self.assertLessEqual(img_min_diff, IMAGE_DIFF_THRESHOLD) @@ -187,7 +188,7 @@ def test_repeatability(self): for n_steps in (0, 5, 20, 10, 0): env.reset(seed=0) if a_prev is None: - a_prev = env.system_state.a.copy() - assert np.all(a_prev == env.system_state.a) + a_prev = env.robot_state.a.copy() + assert np.all(a_prev == env.robot_state.a) for _ in range(n_steps): env.step(env.action) diff --git a/python/gym_jiminy/unit_py/test_pipeline_design.py b/python/gym_jiminy/unit_py/test_pipeline_design.py index d9918afb5..289a9f01e 100644 --- a/python/gym_jiminy/unit_py/test_pipeline_design.py +++ b/python/gym_jiminy/unit_py/test_pipeline_design.py @@ -156,7 +156,7 @@ def test_initial_state(self): imu_data_ref = env.simulator.robot.sensor_measurements['ImuSensor'] imu_data_obs = obs['measurements']['ImuSensor'][-1] self.assertTrue(np.all(imu_data_ref == imu_data_obs)) - state_ref = {'q': env.system_state.q, 'v': env.system_state.v} + state_ref = {'q': env.robot_state.q, 'v': env.robot_state.v} state_obs = obs['states']['agent'] self.assertTrue(np.all(state_ref['q'] == state_obs['q'])) self.assertTrue(np.all(state_ref['v'] == state_obs['v'])) @@ -184,7 +184,7 @@ def test_step_state(self): imu_data_ref = env.simulator.robot.sensor_measurements['ImuSensor'] imu_data_obs = obs['measurements']['ImuSensor'][-1] self.assertFalse(np.all(imu_data_ref == imu_data_obs)) - state_ref = {'q': env.system_state.q, 'v': env.system_state.v} + state_ref = {'q': env.robot_state.q, 'v': env.robot_state.v} state_obs = obs['states']['agent'] self.assertTrue(np.all(state_ref['q'] == state_obs['q'])) self.assertTrue(np.all(state_ref['v'] == state_obs['v'])) @@ -207,6 +207,7 @@ def test_update_periods(self): def configure_telemetry() -> InterfaceJiminyEnv: engine_options = env.simulator.engine.get_options() engine_options['telemetry']['enableCommand'] = True + engine_options['stepper']['logInternalStepperSteps'] = False env.simulator.engine.set_options(engine_options) return env @@ -214,7 +215,7 @@ def configure_telemetry() -> InterfaceJiminyEnv: env.step(env.action) # Check that the command is updated 1/2 low-level controller update - log_vars = env.log_data["variables"] + log_vars = env.log_data['variables'] u_log = log_vars['HighLevelController.currentCommandLF_HAA'] self.assertEqual(env.control_dt, 2 * env.unwrapped.control_dt) self.assertTrue(np.all(u_log[:2] == 0.0)) diff --git a/python/jiminy_py/examples/box_uneven_ground_impulse_contact.py b/python/jiminy_py/examples/box_uneven_ground_impulse_contact.py index 1e590fd25..6d5ee38dd 100644 --- a/python/jiminy_py/examples/box_uneven_ground_impulse_contact.py +++ b/python/jiminy_py/examples/box_uneven_ground_impulse_contact.py @@ -33,9 +33,10 @@ engine_options = simulator.engine.get_options() engine_options['contacts']['model'] = 'constraint' engine_options['contacts']['stabilizationFreq'] = 20.0 - engine_options["constraints"]["regularization"] = 0.0 + engine_options["constraints"]['regularization'] = 0.0 # Configure integrator + engine_options["stepper"]['logInternalStepperSteps'] = True engine_options['stepper']['odeSolver'] = 'runge_kutta_dopri5' engine_options['stepper']['dtMax'] = 1.0e-3 @@ -53,8 +54,8 @@ simulator.engine.set_options(engine_options) # Sample the initial state - qpos = pin.neutral(simulator.system.robot.pinocchio_model) - qvel = np.zeros(simulator.system.robot.nv) + qpos = pin.neutral(simulator.robot.pinocchio_model) + qvel = np.zeros(simulator.robot.nv) qpos[2] += 1.5 qvel[0] = 2.0 qvel[3] = 1.0 diff --git a/python/jiminy_py/examples/collision_detection.py b/python/jiminy_py/examples/collision_detection.py index bbeedd8fe..423fb44d6 100644 --- a/python/jiminy_py/examples/collision_detection.py +++ b/python/jiminy_py/examples/collision_detection.py @@ -11,21 +11,6 @@ # Get script directory MODULE_DIR = os.path.dirname(__file__) -# Create a gym environment for a simple cube -urdf_path = f"{MODULE_DIR}/../../jiminy_py/unit_py/data/box_collision_mesh.urdf" -simulator = Simulator.build(urdf_path, has_freeflyer=True) - -# Sample the initial state -qpos = pin.neutral(simulator.system.robot.pinocchio_model) -qvel = np.zeros(simulator.system.robot.nv) -qpos[2] += 1.5 -qvel[0] = 2.0 -qvel[3] = 1.0 -qvel[5] = 2.0 - -# Run a simulation -simulator.start(qpos, qvel) - # Create collision detection functor class CollisionChecker: def __init__(self, @@ -54,11 +39,27 @@ def __call__(self) -> bool: self.oMg1, self.oMg2, self.req, self.res)) if __name__ == '__main__': + # Create a gym environment for a simple cube + urdf_path = f"{MODULE_DIR}/../../jiminy_py/unit_py/data/box_collision_mesh.urdf" + simulator = Simulator.build(urdf_path, has_freeflyer=True) + + # Instantiate collision checker check_collision = CollisionChecker(simulator.robot.collision_model, simulator.robot.collision_data, "MassBody_0", "ground") + # Sample the initial state + qpos = pin.neutral(simulator.robot.pinocchio_model) + qvel = np.zeros(simulator.robot.nv) + qpos[2] += 1.5 + qvel[0] = 2.0 + qvel[3] = 1.0 + qvel[5] = 2.0 + + # Run a simulation + simulator.start(qpos, qvel) + # Run the simulation until collision detection while True: simulator.step(1e-3) @@ -68,4 +69,3 @@ def __call__(self) -> bool: # Replay the simulation simulator.replay(enable_travelling=False, display_contact_frames=True) - diff --git a/python/jiminy_py/examples/constraint_fixed_frame.py b/python/jiminy_py/examples/constraint_fixed_frame.py index eb78a4b51..87cd7e87b 100644 --- a/python/jiminy_py/examples/constraint_fixed_frame.py +++ b/python/jiminy_py/examples/constraint_fixed_frame.py @@ -18,6 +18,7 @@ urdf_path = f"{MODULE_DIR}/../../jiminy_py/unit_py/data/sphere_primitive.urdf" simulator = Simulator.build( urdf_path, has_freeflyer=True, hardware_path="") + robot = simulator.robot # Disable constraint solver regularization engine_options = simulator.engine.get_options() @@ -33,17 +34,17 @@ # Add fixed frame constraint constraint = jiminy.FrameConstraint( "MassBody", [True, True, True, True, True, True]) - simulator.robot.add_constraint("MassBody", constraint) + robot.add_constraint("MassBody", constraint) constraint.baumgarte_freq = 1.0 # Add IMU to the robot imu_sensor = jiminy.ImuSensor("MassBody") - simulator.robot.attach_sensor(imu_sensor) + robot.attach_sensor(imu_sensor) imu_sensor.initialize("MassBody") # Sample the initial state - qpos = pin.neutral(simulator.robot.pinocchio_model) - qvel = np.zeros(simulator.robot.nv) + qpos = pin.neutral(robot.pinocchio_model) + qvel = np.zeros(robot.nv) # Run a simulation delta = [] @@ -53,7 +54,7 @@ delta_t = simulator.stepper_state.t % (1.1 / constraint.baumgarte_freq) if min(delta_t, 1.1 / constraint.baumgarte_freq - delta_t) < 1e-6: constraint.reference_transform = pin.SE3.Random() - transform = simulator.robot.pinocchio_data.oMf[constraint.frame_index] + transform = robot.pinocchio_data.oMf[constraint.frame_index] ref_transform = constraint.reference_transform delta.append(np.concatenate(( transform.translation - ref_transform.translation, @@ -66,7 +67,7 @@ simulator.render(display_dcm=False) simulator.viewer._backend_obj.gui.show_floor(False) simulator.viewer.add_marker( - "MassBody", "frame", pose=simulator.robot.pinocchio_data.oMf[1]) + "MassBody", "frame", pose=robot.pinocchio_data.oMf[1]) simulator.replay(enable_travelling=False) # Plot the simulation data diff --git a/python/jiminy_py/examples/double_pendulum.py b/python/jiminy_py/examples/double_pendulum.py index 50976367e..e080b92e4 100644 --- a/python/jiminy_py/examples/double_pendulum.py +++ b/python/jiminy_py/examples/double_pendulum.py @@ -36,18 +36,16 @@ def compute_command(self, t, q, v, command): def internal_dynamics(self, t, q, v, u_custom): pass - controller = Controller() - controller.initialize(robot) + robot.controller = Controller() # Instantiate the engine engine = jiminy.Engine() - engine.initialize(robot, controller) + engine.add_robot(robot) # ################## Configuration the simulation ######################### robot_options = robot.get_options() engine_options = engine.get_options() - ctrl_options = controller.get_options() robot_options["telemetry"]["enableImuSensors"] = True engine_options["telemetry"]["isPersistent"] = True @@ -76,7 +74,6 @@ def internal_dynamics(self, t, q, v, u_custom): robot.set_options(robot_options) engine.set_options(engine_options) - controller.set_options(ctrl_options) # ####################### Run the simulation ############################## diff --git a/python/jiminy_py/examples/force_coupling.py b/python/jiminy_py/examples/force_coupling.py index c05676118..e4e30a0ef 100644 --- a/python/jiminy_py/examples/force_coupling.py +++ b/python/jiminy_py/examples/force_coupling.py @@ -18,15 +18,15 @@ urdf_path = f"{MODULE_DIR}/../../jiminy_py/unit_py/data/sphere_primitive.urdf" # Instantiate the robots - robot1 = jiminy.Robot() + robot1 = jiminy.Robot("robot1") robot1.initialize(urdf_path, has_freeflyer=True) - robot2 = jiminy.Robot() + robot2 = jiminy.Robot("robot2") robot2.initialize(urdf_path, has_freeflyer=True) # Instantiate a multi-robot engine - engine = jiminy.EngineMultiRobot() - engine.add_system("robot1", robot1) - engine.add_system("robot2", robot2) + engine = jiminy.Engine() + engine.add_robot(robot1) + engine.add_robot(robot2) # Define coupling force stiffness = np.array([0.2, 0.5, 1.0, 0.2, 0.3, 0.6]) @@ -68,7 +68,7 @@ # Add markers Viewer.close() - views = [Viewer(system.robot) for system in engine.systems] + views = [Viewer(robot) for robot in engine.robots] Viewer._backend_obj.gui.show_floor(False) views[0].add_marker("root_joint_1", "frame", oMf1, color="black", scale=1) views[0].add_marker("root_joint_2", "frame", oMf2, color="red", scale=1) @@ -76,7 +76,7 @@ # Run the simulation while extracting the coupling force dt = 0.01 force_refs = [ - system_state.f_external[1] for system_state in engine.system_states] + robot_state.f_external[1] for robot_state in engine.robot_states] forces, kinetic_momentum, energy_robots, energy_spring = [], [], [], [] try: for i in range(2000): diff --git a/python/jiminy_py/examples/tutorial.ipynb b/python/jiminy_py/examples/tutorial.ipynb index 4291d2a63..c224333d1 100644 --- a/python/jiminy_py/examples/tutorial.ipynb +++ b/python/jiminy_py/examples/tutorial.ipynb @@ -55,7 +55,7 @@ { "data": { "application/vnd.jupyter.widget-view+json": { - "model_id": "56690a443e6f46f5871ef57fc9de0a7b", + "model_id": "b7b5c3fbfecd42118f11583f883bae70", "version_major": 2, "version_minor": 0 }, @@ -92,15 +92,14 @@ "motor.initialize(\"PendulumJoint\")\n", "\n", "# Define the command: for now, the motor is off and doesn't modify the output torque.\n", - "def compute_command(t, q, v, sensors_data, command):\n", + "def compute_command(t, q, v, sensor_measurements, command):\n", " command[:] = 0.0\n", "\n", "# Instantiate and initialize the controller\n", - "controller = jiminy.FunctionalController(compute_command)\n", - "controller.initialize(robot)\n", + "robot.controller = jiminy.FunctionalController(compute_command)\n", "\n", "# Create a simulator using this robot and controller\n", - "simulator = Simulator(robot, controller)\n", + "simulator = Simulator(robot)\n", "\n", "# Set initial condition and simulation length\n", "q0, v0 = np.array([0.1]), np.array([0.0])\n", @@ -125,7 +124,7 @@ { "data": { "application/vnd.jupyter.widget-view+json": { - "model_id": "00500b904eb54f74b1b271b1045e3e04", + "model_id": "441c30fd6bd245bb960e5a9ac62292a4", "version_major": 2, "version_minor": 0 }, @@ -179,7 +178,7 @@ "name": "stderr", "output_type": "stream", "text": [ - "Rendering frames: 100%|██████████| 300/300 [00:01<00:00, 205.47it/s]\n" + "Rendering frames: 100%|██████████| 300/300 [00:01<00:00, 205.96it/s]\n" ] }, { @@ -221,15 +220,11 @@ "Kp = 5000\n", "Kd = 0.05\n", "\n", - "# Create a new controller with Proportional-Derivative command\n", - "def compute_command(t, q, v, sensors_data, command):\n", + "# Define a new controller with Proportional-Derivative command\n", + "def compute_command(t, q, v, sensor_measurements, command):\n", " command[:] = - Kp * (q + Kd * v)\n", "\n", - "controller = jiminy.FunctionalController(compute_command)\n", - "controller.initialize(robot)\n", - "\n", - "# Update the simulator to use the new controller\n", - "simulator.set_controller(controller)" + "robot.controller = jiminy.FunctionalController(compute_command)" ] }, { @@ -250,7 +245,7 @@ { "data": { "application/vnd.jupyter.widget-view+json": { - "model_id": "7d70c0cde6944f64a049fcbd983576d7", + "model_id": "879bb9dcf7df494aaeef19abdd53e261", "version_major": 2, "version_minor": 0 }, @@ -265,7 +260,7 @@ "name": "stderr", "output_type": "stream", "text": [ - "Rendering frames: 100%|██████████| 300/300 [00:01<00:00, 203.59it/s]\n" + "Rendering frames: 100%|██████████| 300/300 [00:01<00:00, 190.82it/s]\n" ] }, { @@ -274,7 +269,7 @@ "\n", " \n", " " ], @@ -295,7 +290,7 @@ " f[1] = 0.0\n", "\n", "# Apply this force profile to a given frame.\n", - "simulator.engine.register_profile_force(\"PendulumMass\", force_profile)\n", + "simulator.register_profile_force(\"PendulumMass\", force_profile)\n", "simulator.simulate(simulation_duration, q0, v0)\n", "\n", "# Replay the simulation with new controller and external forces\n", @@ -319,18 +314,18 @@ { "data": { "application/vnd.jupyter.widget-view+json": { - "model_id": "54f2bc6432d646c9825bccb0e03ccf3a", + "model_id": "79e7b27cf6d24b3ba945806f30e9cc44", "version_major": 2, "version_minor": 0 }, - "image/png": "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", + "image/png": "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", "text/html": [ "\n", "
\n", "
\n", " jiminy\n", "
\n", - " \n", + " \n", "
\n", " " ], diff --git a/python/jiminy_py/src/jiminy_py/log.py b/python/jiminy_py/src/jiminy_py/log.py index 58a444cc1..34cbcd6da 100644 --- a/python/jiminy_py/src/jiminy_py/log.py +++ b/python/jiminy_py/src/jiminy_py/log.py @@ -44,7 +44,7 @@ FieldNested = Union[Dict[str, 'FieldNested'], Sequence['FieldNested'], str] -read_log = jiminy.core.EngineMultiRobot.read_log +read_log = jiminy.core.Engine.read_log @overload @@ -199,9 +199,8 @@ def build_robot_from_log( os.remove(urdf_path) # Load the options - all_options = log_constants[ - ".".join((ENGINE_NAMESPACE, "options"))] - robot.set_options(all_options["system"]["robot"]) + all_options = log_constants[".".join((ENGINE_NAMESPACE, "options"))] + robot.set_options(all_options["robot"]) # Update model in-place. # Note that `__setstate__` re-allocates memory instead of just calling diff --git a/python/jiminy_py/src/jiminy_py/plot.py b/python/jiminy_py/src/jiminy_py/plot.py index c32ff20e9..f6cd267d3 100644 --- a/python/jiminy_py/src/jiminy_py/plot.py +++ b/python/jiminy_py/src/jiminy_py/plot.py @@ -708,7 +708,7 @@ def plot_log(log_data: Dict[str, Any], # Create figure, without closing the existing one figure = TabbedFigure.plot( time, tabs_data, **{ # type: ignore[arg-type] - "plot_method": "plot", **kwargs}) + "plot_method": "plot", "sync_tabs": True, **kwargs}) # Show the figure if appropriate, blocking if necessary if block and not figure.offscreen: diff --git a/python/jiminy_py/src/jiminy_py/simulator.py b/python/jiminy_py/src/jiminy_py/simulator.py index 0d8acaea5..4b5bab847 100644 --- a/python/jiminy_py/src/jiminy_py/simulator.py +++ b/python/jiminy_py/src/jiminy_py/simulator.py @@ -7,13 +7,11 @@ import logging import pathlib import tempfile -import weakref from copy import deepcopy from itertools import chain from functools import partial -from collections import OrderedDict from typing import ( - Any, List, Dict, Optional, Union, Type, Sequence, Iterable, Callable) + Any, List, Dict, Optional, Union, Sequence, Iterable, Callable) import toml import numpy as np @@ -50,25 +48,21 @@ DEFAULT_GROUND_DAMPING = 2.0e3 +ProfileForceFunc = Callable[[float, np.ndarray, np.ndarray, np.ndarray], None] + + class Simulator: - """This class wraps the different submodules of Jiminy, namely the robot, - controller, engine, and viewer, as a single simulation environment. The - user only as to create a robot and associated controller if any, and - give high-level instructions to the simulator. + """Single-robot simulation wrapper providing a unified user-API on top of + the low-level jiminy C++ core library and Python-native modules for 3D + rendering and log data visualization. """ def __init__(self, # pylint: disable=unused-argument robot: jiminy.Robot, - controller: Optional[jiminy.AbstractController] = None, - engine_class: Type[jiminy.Engine] = jiminy.Engine, use_theoretical_model: bool = False, viewer_kwargs: Optional[Dict[str, Any]] = None, **kwargs: Any) -> None: """ :param robot: Jiminy robot already initialized. - :param controller: Jiminy (observer-)controller already initialized. - Optional: None by default. - :param engine_class: Class of engine to use. - Optional: jiminy_py.core.Engine by default. :param use_theoretical_model: Whether the state corresponds to the theoretical model when updating and fetching the robot's state. @@ -86,27 +80,18 @@ def __init__(self, # pylint: disable=unused-argument # Handling of default argument(s) if "robot_name" not in self.viewer_kwargs: - base_name = re.sub('[^A-Za-z0-9_]', '_', robot.name) + model_name = robot.pinocchio_model.name + base_name = re.sub('[^A-Za-z0-9_]', '_', model_name) robot_name = f"{base_name}_{next(tempfile._get_candidate_names())}" self.viewer_kwargs["robot_name"] = robot_name - # Wrap callback in nested function to hide update of progress bar - # Note that a weak reference must be used to avoid circular reference - # resulting in uncollectable object and hence memory leak. - simulator_proxy = weakref.proxy(self) - - def callback_wrapper(simulator_proxy: weakref.ProxyType, - t: float, - *args: Any, - **kwargs: Any) -> None: - if simulator_proxy.__pbar is not None: - simulator_proxy.__pbar.update(t - simulator_proxy.__pbar.n) - simulator_proxy._callback(t, *args, **kwargs) - # Instantiate the low-level Jiminy engine, then initialize it - self.engine = engine_class() - self.engine.initialize( - robot, controller, partial(callback_wrapper, simulator_proxy)) + self.engine = jiminy.Engine() + self.engine.add_robot(robot) + + # Define proxies for convenience + self.robot = robot + self.robot_state = self.engine.robot_states[0] # Create shared memories and python-native attribute for fast access self.stepper_state = self.engine.stepper_state @@ -117,7 +102,7 @@ def callback_wrapper(simulator_proxy: weakref.ProxyType, self._viewers: Sequence[Viewer] = [] # Internal buffer for progress bar management - self.__pbar: Optional[tqdm] = None + self._pbar: Optional[tqdm] = None # Figure holder self._figure: Optional[TabbedFigure] = None @@ -280,14 +265,62 @@ def is_viewer_available(self) -> bool: return (self.viewer is not None and self.viewer.is_open()) # type: ignore[misc] - def _callback(self, - t: float, # pylint: disable=unused-argument - q: np.ndarray, # pylint: disable=unused-argument - v: np.ndarray, # pylint: disable=unused-argument - out: np.ndarray) -> None: - """Callback method for the simulation. + def register_profile_force(self, + frame_name: str, + force_func: ProfileForceFunc, + update_period: float = 0.0) -> None: + r"""Apply an external force profile on a given frame. + + The force can be time- and state-dependent, and may be time-continuous + or updated periodically (Zero-Order Hold). + + :param frame_name: Name of the frame at which to apply the force. + :param force_func: + .. raw:: html + + Force profile as a callable with signature: + + | force_func\( + | **t**: float, + | **q**: np.ndarray, + | **v**: np.ndarray, + | **force**: np.ndarray + | \) -> None + + where `force` corresponds the spatial force in local world aligned + frame, ie its origin is located at application frame but its basis + is aligned with world frame. It is represented as a `np.ndarray` + (Fx, Fy, Fz, Mx, My, Mz) that must be updated in-place. + :param update_period: Update period of the force. It must be set to 0.0 + for time-continuous. Discrete update is strongly + recommended for native Python callables because + evaluating them is so slow that it would slowdown + the whole simulation. There is no issue for C++ + bindings such as `jiminy.RandomPerlinProcess`. + """ + return self.engine.register_profile_force( + "", frame_name, force_func, update_period) + + def register_impulse_force(self, + frame_name: str, + t: float, + dt: float, + force: np.ndarray) -> None: + r"""Apply an external impulse force on a given frame. + + The force starts at the fixed point in time and lasts a given duration. + In the meantime, its profile is square-shaped, ie the force remains + constant. + + :param frame_name: Name of the frame at which to apply the force. + :param t: Time at which to start applying the external force. + :param dt: Duration of the force. + :param force_func: Spatial force in local world aligned frame, ie its + origin is located at application frame but its basis + is aligned with world frame. It is represented as a + `np.ndarray` (Fx, Fy, Fz, Mx, My, Mz). """ - out[()] = True + return self.engine.register_impulse_force("", frame_name, t, dt, force) def seed(self, seed: Union[np.uint32, np.ndarray]) -> None: """Set the seed of the simulation and reset the simulation. @@ -350,7 +383,7 @@ def start(self, # Note that the force vector must be converted to pain list to avoid # copy with external sub-vector. if self.viewer is not None: - self.viewer.f_external = [*self.system_state.f_external][1:] + self.viewer.f_external = [*self.robot_state.f_external][1:] def simulate(self, t_end: float, @@ -358,6 +391,7 @@ def simulate(self, v_init: np.ndarray, a_init: Optional[np.ndarray] = None, is_state_theoretical: bool = True, + callback: Optional[Callable[[], bool]] = None, log_path: Optional[str] = None, show_progress_bar: bool = True) -> None: """Run a simulation, starting from x0=(q0,v0) at t=0 up to tf. @@ -365,13 +399,17 @@ def simulate(self, .. note:: Optionally, log the result of the simulation. - :param t_end: Simulation end time. + :param t_end: Simulation duration. :param q_init: Initial configuration. :param v_init: Initial velocity. :param a_init: Initial acceleration. :param is_state_theoretical: Whether the initial state is associated with the actual or theoretical model of the robot. + :param callback: Callable that can be specified to abort simulation. It + will be evaluated after every simulation step. Abort + if false is returned. + Optional: None by default. :param log_path: Save log data to this location. Disable if None. Note that the format extension '.data' is enforced. Optional, disable by default. @@ -379,28 +417,53 @@ def simulate(self, simulation. None to enable only if available. Optional: None by default. """ - # Show progress bar if requested + # Handling of progress bar if requested if show_progress_bar: - self.__pbar = tqdm(total=t_end, bar_format=( + # Initialize the progress bar + self._pbar = tqdm(total=t_end, bar_format=( "{percentage:3.0f}%|{bar}| {n:.2f}/{total_fmt} " "[{elapsed}<{remaining}]")) + # Define callable responsible for updating the progress bar + def update_progress_bar() -> bool: + """Update progress bar based on current simulation time. + """ + nonlocal self + if self._pbar is not None: + t = self.engine.stepper_state.t + self._pbar.update(t - self._pbar.n) + return True + + # Hijack simulation callback to also update the progress bar + if callback is None: + callback = update_progress_bar + else: + def callback_wrapper(callback: Callable[[], bool]) -> bool: + """Update progress bar based on current simulation time, + then call a given callback function. + """ + nonlocal update_progress_bar + return update_progress_bar() and callback() + + callback = partial(callback_wrapper, self, callback) + # Run the simulation - exception = None + err = None try: self.engine.simulate( - t_end, q_init, v_init, a_init, is_state_theoretical) + t_end, q_init, v_init, a_init, is_state_theoretical, callback) except Exception as e: # pylint: disable=broad-exception-caught - exception = e - finally: # Make sure that the progress bar is properly closed - if show_progress_bar: - assert self.__pbar is not None - self.__pbar.close() - self.__pbar = None + err = e + + # Make sure that the progress bar is properly closed + if show_progress_bar: + assert self._pbar is not None + self._pbar.close() + self._pbar = None # Re-throw exception if not successful - if exception is not None: - raise exception + if err is not None: + raise err # Write log if log_path is not None and self.engine.stepper_state.q: @@ -476,7 +539,7 @@ def render(self, # Share the external force buffer of the viewer with the engine if self.is_simulation_running: - self.viewer.f_external = [*self.system_state.f_external][1:] + self.viewer.f_external = [*self.robot_state.f_external][1:] if self.viewer.backend.startswith('panda3d'): # Enable display of COM, DCM and contact markers by default if @@ -492,12 +555,14 @@ def render(self, # Enable display of external forces by default only for # the joints having an external force registered to it. if "display_f_external" not in viewer_kwargs: + profile_forces, *_ = self.engine.profile_forces.values() force_frames = set( self.robot.pinocchio_model.frames[f.frame_index].parent - for f in self.engine.profile_forces) + for f in profile_forces) + impulse_forces, *_ = self.engine.impulse_forces.values() force_frames |= set( self.robot.pinocchio_model.frames[f.frame_index].parent - for f in self.engine.impulse_forces) + for f in impulse_forces) visibility = self.viewer._display_f_external assert isinstance(visibility, list) for i in force_frames: @@ -652,47 +717,18 @@ def plot(self, return self._figure - def get_controller_options(self) -> dict: - """Getter of the options of Jiminy Controller. - """ - return self.engine.controller.get_options() - - def set_controller_options(self, options: dict) -> None: - """Setter of the options of Jiminy Controller. - """ - self.engine.controller.set_options(options) - def get_options(self) -> Dict[str, Dict[str, Dict[str, Any]]]: - """Get the options of robot (including controller), and engine. + """Get the options of the engine plus the robot (including controller). """ - options: Dict[str, Dict[str, Dict[str, Any]]] = OrderedDict( - system=OrderedDict(robot=OrderedDict(), controller=OrderedDict()), - engine=OrderedDict()) - robot_options = options['system']['robot'] - robot_options_copy = self.robot.get_options() - robot_options['model'] = robot_options_copy['model'] - robot_options['motors'] = robot_options_copy['motors'] - robot_options['sensors'] = robot_options_copy['sensors'] - robot_options['telemetry'] = robot_options_copy['telemetry'] - options['system']['controller'] = self.get_controller_options() - engine_options = options['engine'] - engine_options_copy = self.engine.get_options() - engine_options['stepper'] = engine_options_copy['stepper'] - engine_options['world'] = engine_options_copy['world'] - engine_options['joints'] = engine_options_copy['joints'] - engine_options['constraints'] = engine_options_copy['constraints'] - engine_options['contacts'] = engine_options_copy['contacts'] - engine_options['telemetry'] = engine_options_copy['telemetry'] - return options + return {'engine': self.engine.get_options(), + 'robot': self.robot.get_options()} def set_options(self, options: Dict[str, Dict[str, Dict[str, Any]]]) -> None: """Set the options of robot (including controller), and engine. """ - controller_options = options['system']['controller'] - self.robot.set_options(options['system']['robot']) - self.set_controller_options(controller_options) self.engine.set_options(options['engine']) + self.robot.set_options(options['robot']) def export_options(self, config_path: Optional[Union[str, os.PathLike]] = None diff --git a/python/jiminy_py/unit_py/test_dense_pole.py b/python/jiminy_py/unit_py/test_dense_pole.py index de467b546..3322b2ce5 100644 --- a/python/jiminy_py/unit_py/test_dense_pole.py +++ b/python/jiminy_py/unit_py/test_dense_pole.py @@ -182,7 +182,7 @@ def test_joint_position_limits(self): is_enabled = const.is_enabled for _ in range(int(np.round(t_end / step_dt))): self.simulator.step(step_dt) - theta = self.simulator.system_state.q[0] + theta = self.simulator.robot_state.q[0] if contact_model != 'constraint': continue if self.joint_limit - np.abs(theta) <= 0.0: diff --git a/python/jiminy_py/unit_py/test_double_spring_mass.py b/python/jiminy_py/unit_py/test_double_spring_mass.py index 86e1d7b6a..bc34a7d1b 100644 --- a/python/jiminy_py/unit_py/test_double_spring_mass.py +++ b/python/jiminy_py/unit_py/test_double_spring_mass.py @@ -148,7 +148,7 @@ def external_force(t, q, v, f): nonlocal k_ext f[0] = - k_ext * (q[0] + q[1]) - engine.register_profile_force("SecondMass", external_force) + engine.register_profile_force("", "SecondMass", external_force) # Add the extra external force to second mass self.A[3, :] += np.array([ @@ -189,8 +189,8 @@ def internal_dynamics(t, q, v, sensor_measurements, u_custom): def compute_command(t, q, v, sensor_measurements, command): # Check if local external force is properly computed nonlocal f_local - if engine.is_initialized: - f_ext = engine.system_state.f_external[joint_index].vector + if engine.is_simulation_running: + f_ext = engine.robot_states[0].f_external[joint_index].vector self.assertTrue(np.allclose(f_ext, f_local, atol=TOLERANCE)) # Create and initialize the engine @@ -207,7 +207,7 @@ def external_force(t, q, v, f): f[:3] = R @ f_local[:3] f[3:] = R @ f_local[3:] - engine.register_profile_force("FirstJoint", external_force) + engine.register_profile_force("", "FirstJoint", external_force) # Configure the engine engine_options = engine.get_options() @@ -326,12 +326,11 @@ def test_constraint_external_force(self): ^ \<>\ [O (f)] <><> [M_11] <><> [M_12 (f)] """ - # Build two robots with freeflyers, with a freeflyer and a fixed second + # Build two robots with freeflyer, with a freeflyer and a fixed second # body constraint. - # Rebuild the model with a freeflyer - robots = [jiminy.Robot(), jiminy.Robot()] - engine = jiminy.EngineMultiRobot() + # Instantiate the engine + engine = jiminy.Engine() # Configure the engine engine_options = engine.get_options() @@ -342,22 +341,22 @@ def test_constraint_external_force(self): engine.set_options(engine_options) # Define some internal parameters - system_names = ['FirstSystem', 'SecondSystem'] k = np.array([[100, 50], [80, 120]]) nu = np.array([[0.2, 0.01], [0.05, 0.1]]) k_cross = 100 # Initialize and configure the engine - for i in [0, 1]: + robot_names = ('FirstSystem', 'SecondSystem') + for i, robot_name in enumerate(robot_names): # Load robot - robots[i] = load_urdf_default( - self.urdf_name, self.motor_names, has_freeflyer=True) + robot = load_urdf_default( + self.urdf_name, self.motor_names, True, robot_name) # Apply constraints freeflyer_constraint = jiminy.FrameConstraint("world") - robots[i].add_constraint("world", freeflyer_constraint) + robot.add_constraint("world", freeflyer_constraint) fix_mass_constraint = jiminy.FrameConstraint("SecondMass") - robots[i].add_constraint("fixMass", fix_mass_constraint) + robot.add_constraint("fixMass", fix_mass_constraint) # Create controller class Controller(jiminy.BaseController): @@ -369,11 +368,10 @@ def __init__(self, k, nu): def internal_dynamics(self, t, q, v, u_custom): u_custom[6:] = - self.k * q[7:] - self.nu * v[6:] - controller = Controller(k[i, :], nu[i, :]) - controller.initialize(robots[i]) + robot.controller = Controller(k[i, :], nu[i, :]) - # Add system to engine - engine.add_system(system_names[i], robots[i], controller) + # Add robot to engine + engine.add_robot(robot) # Add coupling force def force(t, q1, v1, q2, v2, f): @@ -387,7 +385,7 @@ def force(t, q1, v1, q2, v2, f): f[1] = + k_cross * (1 + d2) * q2[7] engine.register_coupling_force( - *system_names, "FirstMass", "FirstMass", force) + *robot_names, "FirstMass", "FirstMass", force) # Initialize the whole system. x_init = {} @@ -415,7 +413,7 @@ def force(t, q1, v1, q2, v2, f): x_jiminy_extract = np.hstack([x[:, [7, 8, 15, 16]] for x in x_jiminy]) # Define dynamics of this system - def system_dynamics(t, x): + def dynamics(t, x): # Velocity to position dx = np.zeros(8) dx[:2] = x[2:4] @@ -442,7 +440,7 @@ def system_dynamics(t, x): return dx x0 = np.hstack([x_init[key][[7, 8, 15, 16]] for key in x_init]) - x_python = integrate_dynamics(time, x0, system_dynamics) + x_python = integrate_dynamics(time, x0, dynamics) np.testing.assert_allclose(x_jiminy_extract, x_python, atol=TOLERANCE) diff --git a/python/jiminy_py/unit_py/test_flexible_arm.py b/python/jiminy_py/unit_py/test_flexible_arm.py index bc88deece..e96b862f8 100644 --- a/python/jiminy_py/unit_py/test_flexible_arm.py +++ b/python/jiminy_py/unit_py/test_flexible_arm.py @@ -102,14 +102,9 @@ def setUp(self): # Remove temporary file os.remove(urdf_path) - # Instantiate and initialize a controller doing nothing - noop_controller = jiminy.FunctionalController() - noop_controller.initialize(robot) - # Create a simulator using this robot and controller self.simulator = Simulator( robot, - noop_controller, viewer_kwargs=dict( camera_pose=((0.0, -2.0, 0.0), (np.pi/2, 0.0, 0.0), None) )) @@ -123,9 +118,10 @@ def _read_write_replay_log(self, log_format): t_end = 4.0 # Generate temporary log file + model_name = self.simulator.robot.pinocchio_model.name ext = log_format if log_format != "binary" else "data" fd, log_path = tempfile.mkstemp( - prefix=f"{self.simulator.robot.name}_", suffix=f".{ext}") + prefix=f"{model_name}_", suffix=f".{ext}") os.close(fd) # Run the simulation @@ -135,7 +131,7 @@ def _read_write_replay_log(self, log_format): # Generate temporary video file fd, video_path = tempfile.mkstemp( - prefix=f"{self.simulator.robot.name}_", suffix=".mp4") + prefix=f"{model_name}_", suffix=".mp4") os.close(fd) # Record the result @@ -192,7 +188,7 @@ def test_rigid_vs_flex_at_frame(self): t_end, q0, v0, is_state_theoretical=True, show_progress_bar=False) # Extract the final configuration - q_rigid = self.simulator.system_state.q.copy() + q_rigid = self.simulator.robot_state.q.copy() # Render the scene img_rigid = self.simulator.render(return_rgb_array=True) @@ -225,7 +221,7 @@ def test_rigid_vs_flex_at_frame(self): # Extract the final configuration q_flex.append( self.simulator.robot.get_rigid_configuration_from_flexible( - self.simulator.system_state.q)) + self.simulator.robot_state.q)) # Render the scene img_flex.append(self.simulator.render(return_rgb_array=True)) diff --git a/python/jiminy_py/unit_py/test_multi_robot.py b/python/jiminy_py/unit_py/test_multi_robot.py index e0380beff..7735f33bc 100644 --- a/python/jiminy_py/unit_py/test_multi_robot.py +++ b/python/jiminy_py/unit_py/test_multi_robot.py @@ -46,7 +46,7 @@ def internal_dynamics(self, t, q, v, u_custom): u_custom[:] = - self.k * q - self.nu * v # Create two identical robots - engine = jiminy.EngineMultiRobot() + engine = jiminy.Engine() # Configure the engine engine_options = engine.get_options() @@ -55,35 +55,34 @@ def internal_dynamics(self, t, q, v, u_custom): engine_options["stepper"]["tolRel"] = TOLERANCE * 1e-2 engine.set_options(engine_options) - system_names = ['FirstSystem', 'SecondSystem'] - robots = [] - for i in range(2): - robots.append(load_urdf_default(urdf_name, motor_names)) + robot_names = ('FirstSystem', 'SecondSystem') + for i, robot_name in enumerate(robot_names): + robot = load_urdf_default( + urdf_name, motor_names, False, robot_name) # Create controller - controller = Controller(k[i], nu[i]) - controller.initialize(robots[i]) + robot.controller = Controller(k[i], nu[i]) # Add system to engine. - engine.add_system(system_names[i], robots[i], controller) + engine.add_robot(robot) # Add coupling force between both systems: a spring between both masses def force(t, q1, v1, q2, v2, f): f[0] = k[2] * (q2[0] - q1[0]) + nu[2] * (v2[0] - v1[0]) - engine.register_coupling_force( - system_names[0], system_names[1], "Mass", "Mass", force) + engine.register_coupling_force(*robot_names, "Mass", "Mass", force) # Run simulation and extract some information from log data - x0 = {'FirstSystem': np.array([0.1, 0.0]), - 'SecondSystem': np.array([-0.1, 0.0])} + x0 = dict(zip(robot_names, ( + np.array([0.1, 0.0]), np.array([-0.1, 0.0])))) tf = 10.0 time, x_jiminy = simulate_and_get_state_evolution( engine, tf, x0, split=False) x_jiminy = np.concatenate(x_jiminy, axis=-1) # Define analytical system dynamics: two masses linked by three springs - m = [r.pinocchio_model_th.inertias[1].mass for r in robots] + m = [robot.pinocchio_model_th.inertias[1].mass + for robot in engine.robots] k_eq = [x + k[2] for x in k] nu_eq = [x + nu[2] for x in nu] A = np.array([[ 0.0, 1.0, 0.0, 0.0], diff --git a/python/jiminy_py/unit_py/test_simple_mass.py b/python/jiminy_py/unit_py/test_simple_mass.py index 850eb6f65..dc4d4beb8 100644 --- a/python/jiminy_py/unit_py/test_simple_mass.py +++ b/python/jiminy_py/unit_py/test_simple_mass.py @@ -169,7 +169,7 @@ def _test_collision_and_contact_dynamics(self, shape): ((v_z_jiminy > 0.0) & (penetration_depth < 0.0))))[0]) > 1)) # Compare the numerical and analytical equilibrium state. - f_ext_z = engine.system_state.f_external[joint_index].linear[2] + f_ext_z = engine.robot_states[0].f_external[joint_index].linear[2] self.assertTrue(np.allclose(f_ext_z, weight, atol=TOLERANCE)) self.assertTrue(np.allclose( -penetration_depth[-1], weight / self.k_contact, atol=TOLERANCE)) @@ -200,14 +200,14 @@ def test_contact_sensor(self): def check_sensor_measurements(t, q, v, sensor_measurements, command): # Verify sensor data, if the engine has been initialized nonlocal engine_proxy - if engine_proxy.is_initialized: + if engine_proxy.is_simulation_running: f_linear = sensor_measurements[ ContactSensor.type, self.body_name] f_wrench = sensor_measurements[ ForceSensor.type, self.body_name] f_contact_sensor = frame_pose * Force(f_linear, np.zeros(3)) f_force_sensor = frame_pose * Force(*np.split(f_wrench, 2)) - f_true = engine_proxy.system_state.f_external[joint_index] + f_true = engine_proxy.robot_states[0].f_external[joint_index] self.assertTrue(np.allclose( f_contact_sensor.linear, f_true.linear, atol=TOLERANCE)) self.assertTrue(np.allclose( @@ -265,7 +265,7 @@ def _test_friction_model(self, shape): # Register an impulse of force t0, dt, Fx = 0.05, 0.8, 5.0 wrench = np.array([Fx, 0.0, 0.0, 0.0, 0.0, 0.0]) - engine.register_impulse_force(self.body_name, t0, dt, wrench) + engine.register_impulse_force("", self.body_name, t0, dt, wrench) # Run simulation x0 = neutral_state(robot, split=False) @@ -343,7 +343,7 @@ def test_fixed_frame_constraint(self): # Create, initialize, and configure the engine engine = jiminy.Engine() - engine.initialize(robot) + engine.add_robot(robot) # Disable constraint solver regularization engine_options = engine.get_options() @@ -373,7 +373,7 @@ def test_fixed_frame_constraint(self): transform.translation - ref_transform.translation, log3(transform.rotation @ ref_transform.rotation.T))) self.assertFalse(np.any(delta / delta_prev > 1.0)) - engine.step(dt_desired=0.01) + engine.step(step_dt=0.01) delta_prev = delta engine.stop() diff --git a/python/jiminy_py/unit_py/test_simple_pendulum.py b/python/jiminy_py/unit_py/test_simple_pendulum.py index 4b2d1390d..26c3728f1 100644 --- a/python/jiminy_py/unit_py/test_simple_pendulum.py +++ b/python/jiminy_py/unit_py/test_simple_pendulum.py @@ -68,7 +68,14 @@ def _simulate_and_get_imu_data_evolution( corresponds to a given time. """ # Run simulation - q0, v0 = x0[:engine.robot.nq], x0[-engine.robot.nv:] + if isinstance(x0, np.ndarray): + q0, v0 = x0[:engine.robots[0].nq], x0[-engine.robots[0].nv:] + else: + q0, v0 = {}, {} + for robot in engine.robots: + q0[robot.name] = x0[robot.name][:robot.nq] + v0[robot.name] = x0[robot.name][-robot.nv:] + engine.simulate(tf, q0, v0) # Get log data @@ -397,7 +404,7 @@ def sys(t): "F": np.array([0.0, 0.0, 2.0e5, 0.0, 0.0, 0.0])}] for force in F_register: engine.register_impulse_force( - "PendulumLink", force["t"], force["dt"], force["F"]) + "", "PendulumLink", force["t"], force["dt"], force["F"]) # Configure the engine: No gravity + Continuous time simulation engine_options = engine.get_options() diff --git a/python/jiminy_py/unit_py/utilities.py b/python/jiminy_py/unit_py/utilities.py index 492bec62a..a141113ee 100644 --- a/python/jiminy_py/unit_py/utilities.py +++ b/python/jiminy_py/unit_py/utilities.py @@ -18,7 +18,8 @@ def load_urdf_default(urdf_name: str, motor_names: Sequence[str] = (), - has_freeflyer: bool = False) -> jiminy.Robot: + has_freeflyer: bool = False, + robot_name: str = "") -> jiminy.Robot: """Create a jiminy.Robot from a URDF with several simplifying hypothesis. The goal of this function is to ease creation of `jiminy.Robot` from a URDF @@ -38,7 +39,7 @@ def load_urdf_default(urdf_name: str, urdf_path = os.path.join(data_root_dir, urdf_name) # Create and initialize the robot - robot = jiminy.Robot() + robot = jiminy.Robot(robot_name) robot.initialize(urdf_path, has_freeflyer, [data_root_dir]) # Add motors to the robot @@ -62,7 +63,7 @@ def load_urdf_default(urdf_name: str, def setup_controller_and_engine( - engine: jiminy.EngineMultiRobot, + engine: jiminy.Engine, robot: jiminy.Robot, compute_command: Optional[FunctionalControllerCallable] = None, internal_dynamics: Optional[FunctionalControllerCallable] = None @@ -72,8 +73,8 @@ def setup_controller_and_engine( The goal of this function is to ease the configuration of `jiminy.Engine` by doing the following operations: - - Wrapping the control law and internal dynamics in a - jiminy.FunctionalController. + - Wrapping the control law and internal dynamics as + `jiminy.FunctionalController`. - Register the system robot/controller in the engine to integrate its dynamics. @@ -82,8 +83,7 @@ def setup_controller_and_engine( :param compute_command: .. raw:: html - Control law, which must be an function handle with the following - signature: + Control law as a callable with signature: | compute_command\( | **t**: float, @@ -97,7 +97,7 @@ def setup_controller_and_engine( :param internal_dynamics: .. raw:: html - Internal dynamics function handle with signature: + Internal dynamics as a callable with signature: | internal_dynamics\( | **t**: float, @@ -110,11 +110,11 @@ def setup_controller_and_engine( Optional: No internal dynamics by default. """ # Instantiate the controller - controller = jiminy.FunctionalController(compute_command, internal_dynamics) - controller.initialize(robot) + robot.controller = jiminy.FunctionalController( + compute_command, internal_dynamics) # Initialize the engine - engine.initialize(robot, controller) + engine.add_robot(robot) def neutral_state(robot: jiminy.Model, @@ -174,7 +174,7 @@ def integrate_dynamics(time: np.ndarray, def simulate_and_get_state_evolution( - engine: jiminy.EngineMultiRobot, + engine: jiminy.Engine, tf: float, x0: Union[Dict[str, np.ndarray], np.ndarray], split: bool = False) -> Union[ @@ -193,14 +193,13 @@ def simulate_and_get_state_evolution( given time. """ # Run simulation - if isinstance(engine, jiminy.Engine): - q0, v0 = x0[:engine.robot.nq], x0[-engine.robot.nv:] + if isinstance(x0, np.ndarray): + q0, v0 = x0[:engine.robots[0].nq], x0[-engine.robots[0].nv:] else: q0, v0 = {}, {} - for system in engine.systems: - name = system.name - q0[name] = x0[name][:system.robot.nq] - v0[name] = x0[name][-system.robot.nv:] + for robot in engine.robots: + q0[robot.name] = x0[robot.name][:robot.nq] + v0[robot.name] = x0[robot.name][-robot.nv:] engine.simulate(tf, q0, v0) # Get log data @@ -208,13 +207,13 @@ def simulate_and_get_state_evolution( # Extract state evolution over time time = log_vars['Global.Time'] - if isinstance(engine, jiminy.Engine): + if isinstance(x0, np.ndarray): q_jiminy = np.stack([ log_vars['.'.join(['HighLevelController', s])] - for s in engine.robot.log_position_fieldnames], axis=-1) + for s in engine.robots[0].log_position_fieldnames], axis=-1) v_jiminy = np.stack([ log_vars['.'.join(['HighLevelController', s])] - for s in engine.robot.log_velocity_fieldnames], axis=-1) + for s in engine.robots[0].log_velocity_fieldnames], axis=-1) if split: return time, q_jiminy, v_jiminy else: @@ -222,13 +221,13 @@ def simulate_and_get_state_evolution( return time, x_jiminy else: q_jiminy = [np.stack([ - log_vars['.'.join(['HighLevelController', sys.name, s])] - for s in sys.robot.log_position_fieldnames - ], axis=-1) for sys in engine.systems] + log_vars['.'.join(['HighLevelController', robot.name, s])] + for s in robot.log_position_fieldnames + ], axis=-1) for robot in engine.robots] v_jiminy = [np.stack([ - log_vars['.'.join(['HighLevelController', sys.name, s])] - for s in sys.robot.log_velocity_fieldnames - ], axis=-1) for sys in engine.systems] + log_vars['.'.join(['HighLevelController', robot.name, s])] + for s in robot.log_velocity_fieldnames + ], axis=-1) for robot in engine.robots] if split: return time, q_jiminy, v_jiminy else: diff --git a/python/jiminy_pywrap/include/jiminy/python/engine.h b/python/jiminy_pywrap/include/jiminy/python/engine.h index b3879e4fd..76699add7 100644 --- a/python/jiminy_pywrap/include/jiminy/python/engine.h +++ b/python/jiminy_pywrap/include/jiminy/python/engine.h @@ -8,9 +8,7 @@ namespace jiminy::python { void JIMINY_DLLAPI exposeForces(); void JIMINY_DLLAPI exposeStepperState(); - void JIMINY_DLLAPI exposeSystemState(); - void JIMINY_DLLAPI exposeSystem(); - void JIMINY_DLLAPI exposeEngineMultiRobot(); + void JIMINY_DLLAPI exposeRobotState(); void JIMINY_DLLAPI exposeEngine(); } diff --git a/python/jiminy_pywrap/include/jiminy/python/utilities.h b/python/jiminy_pywrap/include/jiminy/python/utilities.h index feea6419f..6e45586e4 100644 --- a/python/jiminy_pywrap/include/jiminy/python/utilities.h +++ b/python/jiminy_pywrap/include/jiminy/python/utilities.h @@ -253,6 +253,14 @@ namespace jiminy::python #define DEF_READONLY2(namePy, memberFuncPtr) \ DEF_READONLY3(namePy, memberFuncPtr, nullptr) + #define DEF_READONLY_WITH_POLICY4(namePy, attributePtr, policy, doc) \ + add_property(namePy, \ + bp::make_getter(attributePtr, policy), \ + getPropertySignaturesWithDoc(doc, attributePtr).c_str()) + + #define DEF_READONLY_WITH_POLICY3(namePy, attributePtr, policy) \ + DEF_READONLY_WITH_POLICY4(namePy, attributePtr, policy, nullptr) + #define ADD_PROPERTY_GET3(namePy, memberFuncPtr, doc) \ add_property(namePy, \ memberFuncPtr, \ @@ -300,6 +308,7 @@ namespace jiminy::python // Handle overloading #define DEF_READONLY(...) VFUNC(DEF_READONLY, __VA_ARGS__) + #define DEF_READONLY_WITH_POLICY(...) VFUNC(DEF_READONLY_WITH_POLICY, __VA_ARGS__) #define ADD_PROPERTY_GET(...) VFUNC(ADD_PROPERTY_GET, __VA_ARGS__) #define ADD_PROPERTY_GET_WITH_POLICY(...) VFUNC(ADD_PROPERTY_GET_WITH_POLICY, __VA_ARGS__) #define ADD_PROPERTY_GET_SET(...) VFUNC(ADD_PROPERTY_GET_SET, __VA_ARGS__) @@ -806,6 +815,17 @@ namespace jiminy::python }; }; + template + void registerToPythonByValueConverter() + { + bp::type_info info = bp::type_id(); + const bp::converter::registration * reg = bp::converter::registry::query(info); + if (reg == nullptr || *reg->m_to_python == nullptr) + { + bp::to_python_converter, true>(); + } + } + // **************************************************************************** // **************************** PYTHON TO C++ ********************************* // **************************************************************************** @@ -1086,6 +1106,48 @@ namespace jiminy::python boost::apply_visitor(visitor, configField.second); } } + + template + struct RegisterFromPythonByValueConverter + { + RegisterFromPythonByValueConverter() + { + bp::converter::registry::push_back( + &convertible, + &construct, + bp::type_id(), + &bp::converter::expected_from_python_type::get_pytype); + } + + /* No generic implementation for checking whether a Python object is convertible to a given + C++ type can be provided. The only way with the current design is trying to do so by + calling `convertFromPython` and see if it raises an exception, but the cost of this + approach would be prohibitive. */ + static void * convertible(PyObject * obj_ptr); + + static void construct(PyObject * objPtr, + bp::converter::rvalue_from_python_stage1_data * data) + { + // Convert raw python object to boost::python + bp::object objPy = bp::object(bp::handle<>(bp::borrowed(objPtr))); + + // Grab pointer to memory into which to construct the new QString + void * storage = reinterpret_cast *>(data) + ->storage.bytes; + + /* In-place construct the new C++ object from the python object. + Note that, starting with C++17, Return Value Optimization (RVO) is an integral part + of the standard rather than a compiler optimization as it was before. This means + that the following expression is equivalent to constructing the object directly when + calling placement-new operator. No additional temporary is created and neither copy + nor move constructor has to be implemented. They can even be explicitly deleted. + Consequently, this will always compile as long as `convertFromPython` does. */ + new (storage) T{convertFromPython(objPy)}; + + // Stash the memory chunk pointer for later use by boost.python + data->convertible = storage; + } + }; } #endif // UTILITIES_PYTHON_H diff --git a/python/jiminy_pywrap/src/engine.cc b/python/jiminy_pywrap/src/engine.cc index e266cba3c..877ec6fdc 100644 --- a/python/jiminy_pywrap/src/engine.cc +++ b/python/jiminy_pywrap/src/engine.cc @@ -6,7 +6,7 @@ #include "jiminy/core/control/abstract_controller.h" #include "jiminy/core/robot/robot.h" #include "jiminy/core/engine/engine.h" -#include "jiminy/core/engine/engine_multi_robot.h" +#include "jiminy/core/engine/engine.h" #include "pinocchio/bindings/python/fwd.hpp" #include @@ -73,10 +73,10 @@ namespace jiminy::python bp::class_, boost::noncopyable>("CouplingForce", bp::no_init) - .DEF_READONLY("system_name_1", &CouplingForce::systemName1) - .DEF_READONLY("system_index_1", &CouplingForce::systemIndex1) - .DEF_READONLY("system_name_2", &CouplingForce::systemName2) - .DEF_READONLY("system_index_2", &CouplingForce::systemIndex2) + .DEF_READONLY("robot_name_1", &CouplingForce::robotName1) + .DEF_READONLY("robot_index_1", &CouplingForce::robotIndex1) + .DEF_READONLY("robot_name_2", &CouplingForce::robotName2) + .DEF_READONLY("robot_index_2", &CouplingForce::robotIndex2) .ADD_PROPERTY_GET("func", couplingForceWrapper); bp::class_ + struct PyRobotStateVisitor : public bp::def_visitor { public: /// \brief Expose C++ API through the visitor. @@ -173,21 +173,21 @@ namespace jiminy::python { // clang-format off cl - .DEF_READONLY("q", &SystemState::q) - .DEF_READONLY("v", &SystemState::v) - .DEF_READONLY("a", &SystemState::a) - .DEF_READONLY("command", &SystemState::command) - .DEF_READONLY("u", &SystemState::u) - .DEF_READONLY("u_motor", &SystemState::uMotor) - .DEF_READONLY("u_internal", &SystemState::uInternal) - .DEF_READONLY("u_custom", &SystemState::uCustom) - .DEF_READONLY("f_external", &SystemState::fExternal) - .def("__repr__", &PySystemStateVisitor::repr) + .DEF_READONLY("q", &RobotState::q) + .DEF_READONLY("v", &RobotState::v) + .DEF_READONLY("a", &RobotState::a) + .DEF_READONLY("command", &RobotState::command) + .DEF_READONLY("u", &RobotState::u) + .DEF_READONLY("u_motor", &RobotState::uMotor) + .DEF_READONLY("u_internal", &RobotState::uInternal) + .DEF_READONLY("u_custom", &RobotState::uCustom) + .DEF_READONLY("f_external", &RobotState::fExternal) + .def("__repr__", &PyRobotStateVisitor::repr) ; // clang-format on } - static std::string repr(SystemState & self) + static std::string repr(RobotState & self) { std::stringstream s; Eigen::IOFormat HeavyFmt(5, 1, ", ", "", "", "", "[", "]\n"); @@ -211,52 +211,19 @@ namespace jiminy::python static void expose() { // clang-format off - bp::class_, - boost::noncopyable>("SystemState", bp::no_init) - .def(PySystemStateVisitor()); + bp::class_, + boost::noncopyable>("RobotState", bp::no_init) + .def(PyRobotStateVisitor()); // clang-format on } }; - BOOST_PYTHON_VISITOR_EXPOSE(SystemState) - - // ***************************** PySystemVisitor *********************************** + BOOST_PYTHON_VISITOR_EXPOSE(RobotState) - struct PySystemVisitor : public bp::def_visitor - { - public: - /// \brief Expose C++ API through the visitor. - template - void visit(PyClass & cl) const - { - // clang-format off - cl - .DEF_READONLY("name", &System::name) - .DEF_READONLY("robot", &System::robot) - .DEF_READONLY("controller", &System::controller) - .DEF_READONLY("callbackFct", &System::callback) - ; - // clang-format on - } + // ************************* PyEngineVisitor **************************** - static void expose() - { - // clang-format off - bp::class_("System", bp::no_init) - .def(PySystemVisitor()); - - bp::class_>("SystemVector", bp::no_init) - .def(vector_indexing_suite_no_contains>()); - // clang-format on - } - }; - - BOOST_PYTHON_VISITOR_EXPOSE(System) - - // ************************* PyEngineMultiRobotVisitor **************************** - - struct PyEngineMultiRobotVisitor : public bp::def_visitor + struct PyEngineVisitor : public bp::def_visitor { public: template @@ -264,40 +231,46 @@ namespace jiminy::python { // clang-format off cl - .def("add_system", &PyEngineMultiRobotVisitor::addSystem, - (bp::arg("self"), "system_name", "robot", - bp::arg("controller") = bp::object(), - bp::arg("callback_function") = bp::object())) - .def("remove_system", &EngineMultiRobot::removeSystem, - (bp::arg("self"), "system_name")) - .def("set_controller", &EngineMultiRobot::setController, - (bp::arg("self"), "system_name", "controller")) + .def("add_robot", &Engine::addRobot, + (bp::arg("self"), "robot")) + .def("remove_robot", &Engine::removeRobot, + (bp::arg("self"), "robot_name")) .def("reset", static_cast< - void (EngineMultiRobot::*)(bool, bool) - >(&EngineMultiRobot::reset), + void (Engine::*)(bool, bool) + >(&Engine::reset), (bp::arg("self"), bp::arg("reset_random_generator") = false, bp::arg("remove_all_forces") = false)) - .def("start", &PyEngineMultiRobotVisitor::start, - (bp::arg("self"), "q_init_list", "v_init_list", - bp::arg("a_init_list") = bp::object())) // bp::object() means 'None' in Python - .def("step", &PyEngineMultiRobotVisitor::step, - (bp::arg("self"), bp::arg("dt_desired") = -1)) - .def("stop", &EngineMultiRobot::stop, (bp::arg("self"))) - .def("simulate", &PyEngineMultiRobotVisitor::simulate, - (bp::arg("self"), "t_end", "q_init_list", "v_init_list", - bp::arg("a_init_list") = bp::object())) - .def("compute_forward_kinematics", &EngineMultiRobot::computeForwardKinematics, - (bp::arg("system"), "q", "v", "a")) + .def("start", &PyEngineVisitor::startFromDict, + (bp::arg("self"), "q_init_dict", "v_init_dict", + bp::arg("a_init_dict") = bp::object())) // bp::object() means 'None' in Python + .def("start", &PyEngineVisitor::start, + (bp::arg("self"), "q_init", "v_init", + bp::arg("a_init") = bp::object(), + bp::arg("is_state_theoretical") = false)) + .def("step", &Engine::step, + (bp::arg("self"), bp::arg("step_dt") = -1)) + .def("stop", &Engine::stop, (bp::arg("self"))) + .def("simulate", &PyEngineVisitor::simulateFromDict, + (bp::arg("self"), "t_end", "q_init_dict", "v_init_dict", + bp::arg("a_init_dict") = bp::object(), + bp::arg("callback") = bp::object())) + .def("simulate", &PyEngineVisitor::simulate, + (bp::arg("self"), "t_end", "q_init", "v_init", + bp::arg("a_init") = bp::object(), + bp::arg("is_state_theoretical") = false, + bp::arg("callback") = bp::object())) + .def("compute_forward_kinematics", &Engine::computeForwardKinematics, + (bp::arg("robot"), "q", "v", "a")) .staticmethod("compute_forward_kinematics") - .def("compute_systems_dynamics", &PyEngineMultiRobotVisitor::computeSystemsDynamics, + .def("compute_robots_dynamics", &PyEngineVisitor::computeRobotsDynamics, bp::return_value_policy>(), (bp::arg("self"), "t_end", "q_list", "v_list")) - .ADD_PROPERTY_GET("log_data", &PyEngineMultiRobotVisitor::getLog) - .def("read_log", &PyEngineMultiRobotVisitor::readLog, + .ADD_PROPERTY_GET("log_data", &PyEngineVisitor::getLog) + .def("read_log", &PyEngineVisitor::readLog, (bp::arg("fullpath"), bp::arg("format") = bp::object()), "Read a logfile from jiminy.\n\n" ".. note::\n This function supports both binary and hdf5 log.\n\n" @@ -305,47 +278,47 @@ namespace jiminy::python ":param format: Name of the file to load.\n\n" ":returns: Dictionary containing the logged constants and variables.") .staticmethod("read_log") - .def("write_log", &EngineMultiRobot::writeLog, (bp::arg("self"), "fullpath", "format")) + .def("write_log", &Engine::writeLog, (bp::arg("self"), "fullpath", "format")) - .def("register_impulse_force", &PyEngineMultiRobotVisitor::registerImpulseForce, - (bp::arg("self"), "system_name", + .def("register_impulse_force", &PyEngineVisitor::registerImpulseForce, + (bp::arg("self"), "robot_name", "frame_name", "t", "dt", "force")) .def("remove_impulse_forces", static_cast< - void (EngineMultiRobot::*)(const std::string &) - >(&EngineMultiRobot::removeImpulseForces), - (bp::arg("self"), "system_name")) + void (Engine::*)(const std::string &) + >(&Engine::removeImpulseForces), + (bp::arg("self"), "robot_name")) .def("remove_impulse_forces", static_cast< - void (EngineMultiRobot::*)(void) - >(&EngineMultiRobot::removeImpulseForces), + void (Engine::*)(void) + >(&Engine::removeImpulseForces), (bp::arg("self"))) - .ADD_PROPERTY_GET("impulse_forces", &PyEngineMultiRobotVisitor::getImpulseForces) + .ADD_PROPERTY_GET("impulse_forces", &PyEngineVisitor::getImpulseForces) - .def("register_profile_force", &PyEngineMultiRobotVisitor::registerProfileForce, - (bp::arg("self"), "system_name", - "frame_name", "force_function", + .def("register_profile_force", &PyEngineVisitor::registerProfileForce, + (bp::arg("self"), "robot_name", + "frame_name", "force_func", bp::arg("update_period") = 0.0)) .def("remove_profile_forces", static_cast< - void (EngineMultiRobot::*)(const std::string &) - >(&EngineMultiRobot::removeProfileForces), - (bp::arg("self"), "system_name")) + void (Engine::*)(const std::string &) + >(&Engine::removeProfileForces), + (bp::arg("self"), "robot_name")) .def("remove_profile_forces", static_cast< - void (EngineMultiRobot::*)(void) - >(&EngineMultiRobot::removeProfileForces), + void (Engine::*)(void) + >(&Engine::removeProfileForces), (bp::arg("self"))) - .ADD_PROPERTY_GET("profile_forces", &PyEngineMultiRobotVisitor::getProfileForces) + .ADD_PROPERTY_GET("profile_forces", &PyEngineVisitor::getProfileForces) - .def("register_coupling_force", &PyEngineMultiRobotVisitor::registerCouplingForce, + .def("register_coupling_force", &PyEngineVisitor::registerCouplingForce, (bp::arg("self"), - "system_name_1", "system_name_2", + "robot_name_1", "robot_name_2", "frame_name_1", "frame_name_2", - "force_function")) + "force_func")) .def("register_viscoelastic_coupling_force", static_cast< - void (EngineMultiRobot::*)( + void (Engine::*)( const std::string &, const std::string &, const std::string &, @@ -353,24 +326,24 @@ namespace jiminy::python const Vector6d &, const Vector6d &, double) - >(&EngineMultiRobot::registerViscoelasticCouplingForce), - (bp::arg("self"), "system_name_1", "system_name_2", + >(&Engine::registerViscoelasticCouplingForce), + (bp::arg("self"), "robot_name_1", "robot_name_2", "frame_name_1", "frame_name_2", "stiffness", "damping", bp::arg("alpha") = 0.5)) .def("register_viscoelastic_coupling_force", static_cast< - void (EngineMultiRobot::*)( + void (Engine::*)( const std::string &, const std::string &, const std::string &, const Vector6d &, const Vector6d &, double) - >(&EngineMultiRobot::registerViscoelasticCouplingForce), - (bp::arg("self"), "system_name", "frame_name_1", "frame_name_2", + >(&Engine::registerViscoelasticCouplingForce), + (bp::arg("self"), "robot_name", "frame_name_1", "frame_name_2", "stiffness", "damping", bp::arg("alpha") = 0.5)) .def("register_viscoelastic_directional_coupling_force", static_cast< - void (EngineMultiRobot::*)( + void (Engine::*)( const std::string &, const std::string &, const std::string &, @@ -378,140 +351,114 @@ namespace jiminy::python double, double, double) - >(&EngineMultiRobot::registerViscoelasticDirectionalCouplingForce), - (bp::arg("self"), "system_name_1", "system_name_2", "frame_name_1", "frame_name_2", + >(&Engine::registerViscoelasticDirectionalCouplingForce), + (bp::arg("self"), "robot_name_1", "robot_name_2", "frame_name_1", "frame_name_2", "stiffness", "damping", bp::arg("rest_length") = 0.0)) .def("register_viscoelastic_directional_coupling_force", static_cast< - void (EngineMultiRobot::*)( + void (Engine::*)( const std::string &, const std::string &, const std::string &, double, double, double) - >(&EngineMultiRobot::registerViscoelasticDirectionalCouplingForce), - (bp::arg("self"), "system_name", "frame_name_1", "frame_name_2", + >(&Engine::registerViscoelasticDirectionalCouplingForce), + (bp::arg("self"), "robot_name", "frame_name_1", "frame_name_2", "stiffness", "damping", bp::arg("rest_length") = 0.0)) .def("remove_coupling_forces", static_cast< - void (EngineMultiRobot::*)(const std::string &, const std::string &) - >(&EngineMultiRobot::removeCouplingForces), - (bp::arg("self"), "system_name_1", "system_name_2")) + void (Engine::*)(const std::string &, const std::string &) + >(&Engine::removeCouplingForces), + (bp::arg("self"), "robot_name_1", "robot_name_2")) .def("remove_coupling_forces", static_cast< - void (EngineMultiRobot::*)(const std::string &) - >(&EngineMultiRobot::removeCouplingForces), - (bp::arg("self"), "system_name")) + void (Engine::*)(const std::string &) + >(&Engine::removeCouplingForces), + (bp::arg("self"), "robot_name")) .def("remove_coupling_forces", static_cast< - void (EngineMultiRobot::*)(void) - >(&EngineMultiRobot::removeCouplingForces), + void (Engine::*)(void) + >(&Engine::removeCouplingForces), (bp::arg("self"))) .ADD_PROPERTY_GET_WITH_POLICY("coupling_forces", - &EngineMultiRobot::getCouplingForces, + &Engine::getCouplingForces, bp::return_value_policy>()) - .def("remove_all_forces", &EngineMultiRobot::removeAllForces) + .def("remove_all_forces", &Engine::removeAllForces) + + .def("set_options", &PyEngineVisitor::setOptions) + .def("get_options", &Engine::getOptions) - .def("set_options", &PyEngineMultiRobotVisitor::setOptions) - .def("get_options", &EngineMultiRobot::getOptions) + .DEF_READONLY_WITH_POLICY("robots", + &Engine::robots_, + bp::return_value_policy>()) - .DEF_READONLY("systems", &EngineMultiRobot::systems_) - .ADD_PROPERTY_GET_WITH_POLICY("system_names", - &EngineMultiRobot::getSystemNames, - bp::return_value_policy>()) - .ADD_PROPERTY_GET("system_states", &PyEngineMultiRobotVisitor::getSystemStates) + .ADD_PROPERTY_GET("robot_states", &PyEngineVisitor::getRobotStates) .ADD_PROPERTY_GET_WITH_POLICY("stepper_state", - &EngineMultiRobot::getStepperState, + &Engine::getStepperState, bp::return_value_policy>()) .ADD_PROPERTY_GET_WITH_POLICY("is_simulation_running", - &EngineMultiRobot::getIsSimulationRunning, + &Engine::getIsSimulationRunning, bp::return_value_policy>()) - .add_static_property("simulation_duration_max", &EngineMultiRobot::getSimulationDurationMax) - .add_static_property("telemetry_time_unit", &EngineMultiRobot::getTelemetryTimeUnit) + .add_static_property("simulation_duration_max", &Engine::getSimulationDurationMax) + .add_static_property("telemetry_time_unit", &Engine::getTelemetryTimeUnit) ; // clang-format on } - static void addSystem(EngineMultiRobot & self, - const std::string & systemName, - const std::shared_ptr & robot, - const bp::object & controllerPy, - const bp::object & callbackPy) - { - AbortSimulationFunction callback; - if (callbackPy.is_none()) - { - callback = [](double /* t */, - const Eigen::VectorXd & /* q */, - const Eigen::VectorXd & /* v */) -> bool - { - return true; - }; - } - else - { - callback = TimeStateFunPyWrapper(callbackPy); - } - if (!controllerPy.is_none()) - { - const std::shared_ptr controller = - bp::extract>(controllerPy); - return self.addSystem(systemName, robot, controller, callback); - } - return self.addSystem(systemName, robot, callback); - } - - static bp::dict getImpulseForces(EngineMultiRobot & self) + static bp::dict getImpulseForces(Engine & self) { bp::dict impulseForcesPy; - for (const auto & systemName : self.getSystemNames()) + for (const auto & robot : self.robots_) { - const ImpulseForceVector & impulseForces = self.getImpulseForces(systemName); - impulseForcesPy[systemName] = convertToPython(impulseForces, false); + const std::string & robotName = robot->getName(); + const ImpulseForceVector & impulseForces = self.getImpulseForces(robotName); + impulseForcesPy[robotName] = convertToPython(impulseForces, false); } return impulseForcesPy; } - static bp::dict getProfileForces(EngineMultiRobot & self) + static bp::dict getProfileForces(Engine & self) { bp::dict profileForcessPy; - for (const auto & systemName : self.getSystemNames()) + for (const auto & robot : self.robots_) { - const ProfileForceVector & profileForces = self.getProfileForces(systemName); - profileForcessPy[systemName] = convertToPython(profileForces, false); + const std::string & robotName = robot->getName(); + const ProfileForceVector & profileForces = self.getProfileForces(robotName); + profileForcessPy[robotName] = convertToPython(profileForces, false); } return profileForcessPy; } - static bp::list getSystemStates(EngineMultiRobot & self) + static bp::list getRobotStates(Engine & self) { bp::list systemStates; - for (const std::string & systemName : self.getSystemNames()) + for (const auto & robot : self.robots_) { - const SystemState & systemState = self.getSystemState(systemName); + const std::string & robotName = robot->getName(); + const RobotState & systemState = self.getRobotState(robotName); systemStates.append(convertToPython(systemState, false)); } return systemStates; } - static void registerCouplingForce(EngineMultiRobot & self, - const std::string & systemName1, - const std::string & systemName2, + static void registerCouplingForce(Engine & self, + const std::string & robotName1, + const std::string & robotName2, const std::string & frameName1, const std::string & frameName2, const bp::object & forceFuncPy) { TimeBistateFunPyWrapper forceFunc(forceFuncPy); return self.registerCouplingForce( - systemName1, systemName2, frameName1, frameName2, forceFunc); + robotName1, robotName2, frameName1, frameName2, forceFunc); } - static void start(EngineMultiRobot & self, - const bp::dict & qInitPy, - const bp::dict & vInitPy, - const bp::object & aInitPy) + static void startFromDict(Engine & self, + const bp::dict & qInitPy, + const bp::dict & vInitPy, + const bp::object & aInitPy) { std::optional> aInit = std::nullopt; if (!aInitPy.is_none()) @@ -523,23 +470,37 @@ namespace jiminy::python aInit); } - static void step(EngineMultiRobot & self, double dtDesired) + static void start(Engine & self, + const Eigen::VectorXd & qInit, + const Eigen::VectorXd & vInit, + const bp::object & aInitPy, + bool isStateTheoretical) { - // Only way to handle C++ default values that are not accessible in Python - return self.step(dtDesired); + std::optional aInit = std::nullopt; + if (!aInitPy.is_none()) + { + aInit.emplace(convertFromPython(aInitPy)); + } + return self.start(qInit, vInit, aInit, isStateTheoretical); } - static void simulate(EngineMultiRobot & self, - double endTime, - const bp::dict & qInitPy, - const bp::dict & vInitPy, - const bp::object & aInitPy) + static void simulateFromDict(Engine & self, + double endTime, + const bp::dict & qInitPy, + const bp::dict & vInitPy, + const bp::object & aInitPy, + const bp::object & callbackPy) { std::optional> aInit = std::nullopt; if (!aInitPy.is_none()) { aInit.emplace(convertFromPython>(aInitPy)); } + std::optional callback; + if (!callbackPy.is_none()) + { + callback.emplace(callbackPy); + } return self.simulate( endTime, convertFromPython>(qInitPy), @@ -547,38 +508,65 @@ namespace jiminy::python aInit); } - static std::vector computeSystemsDynamics(EngineMultiRobot & self, - double endTime, - const bp::object & qSplitPy, - const bp::object & vSplitPy) + static void simulate(Engine & self, + double endTime, + const Eigen::VectorXd & qInit, + const Eigen::VectorXd & vInit, + const bp::object & aInitPy, + bool isStateTheoretical, + const bp::object & callbackPy) + { + std::optional aInit = std::nullopt; + if (!aInitPy.is_none()) + { + aInit.emplace(convertFromPython(aInitPy)); + } + AbortSimulationFunction callback; + if (!callbackPy.is_none()) + { + callback = callbackPy; + } + else + { + callback = []() + { + return true; + }; + } + return self.simulate(endTime, qInit, vInit, aInit, isStateTheoretical, callback); + } + + static std::vector computeRobotsDynamics(Engine & self, + double endTime, + const bp::object & qSplitPy, + const bp::object & vSplitPy) { std::vector aSplit; - self.computeSystemsDynamics(endTime, - convertFromPython>(qSplitPy), - convertFromPython>(vSplitPy), - aSplit); + self.computeRobotsDynamics(endTime, + convertFromPython>(qSplitPy), + convertFromPython>(vSplitPy), + aSplit); return aSplit; } - static void registerImpulseForce(EngineMultiRobot & self, - const std::string & systemName, + static void registerImpulseForce(Engine & self, + const std::string & robotName, const std::string & frameName, double t, double dt, const Vector6d & force) { - return self.registerImpulseForce( - systemName, frameName, t, dt, pinocchio::Force{force}); + return self.registerImpulseForce(robotName, frameName, t, dt, pinocchio::Force{force}); } - static void registerProfileForce(EngineMultiRobot & self, - const std::string & systemName, + static void registerProfileForce(Engine & self, + const std::string & robotName, const std::string & frameName, const bp::object & forceFuncPy, double updatePeriod) { TimeStateFunPyWrapper forceFunc(forceFuncPy); - return self.registerProfileForce(systemName, frameName, forceFunc, updatePeriod); + return self.registerProfileForce(robotName, frameName, forceFunc, updatePeriod); } static bp::dict formatLogData(const LogData & logData) @@ -738,7 +726,7 @@ namespace jiminy::python return logDataPy; } - static bp::dict getLog(EngineMultiRobot & self) + static bp::dict getLog(Engine & self) { /* It is impossible to use static boost::python variables. Indeed, the global/static destructor is called after finalization of Python runtime, the later being required @@ -809,11 +797,11 @@ namespace jiminy::python "automatically. Please specify it manually."); } } - const LogData logData = EngineMultiRobot::readLog(filename, format); + const LogData logData = Engine::readLog(filename, format); return formatLogData(logData); } - static void setOptions(EngineMultiRobot & self, const bp::dict & configPy) + static void setOptions(Engine & self, const bp::dict & configPy) { GenericConfig config = self.getOptions(); convertFromPython(configPy, config); @@ -823,201 +811,7 @@ namespace jiminy::python static void expose() { // clang-format off - bp::class_, - boost::noncopyable>("EngineMultiRobot") - .def(PyEngineMultiRobotVisitor()); - // clang-format on - } - }; - - BOOST_PYTHON_VISITOR_EXPOSE(EngineMultiRobot) - - // ***************************** PyEngineVisitor *********************************** - - struct PyEngineVisitor : public bp::def_visitor - { - public: - /// \brief Expose C++ API through the visitor. - template - void visit(PyClass & cl) const - { - // clang-format off - cl - .def("add_system", &Engine::addSystem) - .def("remove_system", &Engine::removeSystem, - (bp::arg("self"), "system_name")) - - .def("initialize", &PyEngineVisitor::initialize, - (bp::arg("self"), "robot", - bp::arg("controller") = std::shared_ptr(), - bp::arg("callback_function") = bp::object())) - .def("set_controller", - static_cast< - void (Engine::*)(std::shared_ptr) - >(&Engine::setController), - (bp::arg("self"), "controller")) - - .def("start", - &PyEngineVisitor::start, - (bp::arg("self"), "q_init", "v_init", - bp::arg("a_init") = bp::object(), - bp::arg("is_state_theoretical") = false)) - .def("simulate", - &PyEngineVisitor::simulate, - (bp::arg("self"), "t_end", "q_init", "v_init", - bp::arg("a_init") = bp::object(), - bp::arg("is_state_theoretical") = false)) - - .def("register_impulse_force", &PyEngineVisitor::registerImpulseForce, - (bp::arg("self"), "frame_name", "t", "dt", "force")) - .ADD_PROPERTY_GET_WITH_POLICY("impulse_forces", - static_cast< - const ImpulseForceVector & (Engine::*)(void) const - >(&Engine::getImpulseForces), - bp::return_value_policy>()) - - .def("register_profile_force", &PyEngineVisitor::registerProfileForce, - (bp::arg("self"), "frame_name", "force_function", - bp::arg("update_period") = 0.0)) - .ADD_PROPERTY_GET_WITH_POLICY("profile_forces", - static_cast< - const ProfileForceVector & (Engine::*)(void) const - >(&Engine::getProfileForces), - bp::return_value_policy>()) - - .def("register_coupling_force", &PyEngineVisitor::registerCouplingForce, - (bp::arg("self"), "frame_name_1", "frame_name_2", "force_function")) - .def("register_viscoelastic_coupling_force", - static_cast< - void (Engine::*)( - const std::string &, - const std::string &, - const Vector6d &, - const Vector6d &, - double) - >(&Engine::registerViscoelasticCouplingForce), - (bp::arg("self"), "frame_name_1", "frame_name_2", "stiffness", "damping", bp::arg("alpha") = 0.5)) - .def("register_viscoelastic_directional_coupling_force", - static_cast< - void (Engine::*)( - const std::string &, - const std::string &, - double, - double, - double) - >(&Engine::registerViscoelasticDirectionalCouplingForce), - (bp::arg("self"), "frame_name_1", "frame_name_2", "stiffness", "damping", - bp::arg("rest_length") = 0.0)) - - .ADD_PROPERTY_GET_WITH_POLICY("is_initialized", - &Engine::getIsInitialized, - bp::return_value_policy()) - .ADD_PROPERTY_GET_WITH_POLICY("system", - &Engine::getSystem, - bp::return_value_policy>()) - .ADD_PROPERTY_GET("robot", &Engine::getRobot) - .ADD_PROPERTY_GET("controller", &Engine::getController) - .ADD_PROPERTY_GET_WITH_POLICY("stepper_state", - &Engine::getStepperState, - bp::return_value_policy>()) - .ADD_PROPERTY_GET_WITH_POLICY("system_state", - &Engine::getSystemState, - bp::return_value_policy>()) - ; - // clang-format on - } - - static void initialize(Engine & self, - const std::shared_ptr & robot, - const std::shared_ptr & controller, - const bp::object & callbackPy) - { - if (callbackPy.is_none()) - { - AbortSimulationFunction callback = [](double /* t */, - const Eigen::VectorXd & /* q */, - const Eigen::VectorXd & /* v */) -> bool - { - return true; - }; - if (controller) - { - return self.initialize(robot, controller, callback); - } - return self.initialize(robot, callback); - } - else - { - TimeStateFunPyWrapper callback(callbackPy); - if (controller) - { - return self.initialize(robot, controller, callback); - } - return self.initialize(robot, callback); - } - } - - static void registerImpulseForce(Engine & self, - const std::string & frameName, - double t, - double dt, - const Vector6d & force) - { - return self.registerImpulseForce(frameName, t, dt, pinocchio::Force{force}); - } - - static void registerProfileForce(Engine & self, - const std::string & frameName, - const bp::object & forceFuncPy, - double updatePeriod) - { - TimeStateFunPyWrapper forceFunc(forceFuncPy); - return self.registerProfileForce(frameName, forceFunc, updatePeriod); - } - - static void registerCouplingForce(Engine & self, - const std::string & frameName1, - const std::string & frameName2, - const bp::object & forceFuncPy) - { - TimeStateFunPyWrapper forceFunc(forceFuncPy); - return self.registerCouplingForce(frameName1, frameName2, forceFunc); - } - - static void start(Engine & self, - const Eigen::VectorXd & qInit, - const Eigen::VectorXd & vInit, - const bp::object & aInitPy, - bool isStateTheoretical) - { - std::optional aInit = std::nullopt; - if (!aInitPy.is_none()) - { - aInit.emplace(convertFromPython(aInitPy)); - } - return self.start(qInit, vInit, aInit, isStateTheoretical); - } - - static void simulate(Engine & self, - double endTime, - const Eigen::VectorXd & qInit, - const Eigen::VectorXd & vInit, - const bp::object & aInitPy, - bool isStateTheoretical) - { - std::optional aInit = std::nullopt; - if (!aInitPy.is_none()) - { - aInit.emplace(convertFromPython(aInitPy)); - } - return self.simulate(endTime, qInit, vInit, aInit, isStateTheoretical); - } - - static void expose() - { - // clang-format off - bp::class_, + bp::class_, boost::noncopyable>("Engine") .def(PyEngineVisitor()); diff --git a/python/jiminy_pywrap/src/helpers.cc b/python/jiminy_pywrap/src/helpers.cc index cbb4ec8b2..ad2c93fd4 100644 --- a/python/jiminy_pywrap/src/helpers.cc +++ b/python/jiminy_pywrap/src/helpers.cc @@ -306,33 +306,33 @@ namespace jiminy::python // Do not use EigenPy To-Python converter because it considers matrices with 1 column as vectors bp::def("interpolate_positions", &interpolatePositions, - (bp::arg("pinocchio_model"), "times_in", "positions_in", "times_out"), - bp::return_value_policy>()); + bp::return_value_policy>(), + (bp::arg("pinocchio_model"), "times_in", "positions_in", "times_out")); bp::def("aba", &pinocchio_overload::aba< double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd, pinocchio::Force>, + bp::return_value_policy>(), (bp::arg("pinocchio_model"), "pinocchio_data", "q", "v", "u", "fext"), - "Compute ABA with external forces, store the result in Data::ddq and return it.", - bp::return_value_policy>()); + "Compute ABA with external forces, store the result in Data::ddq and return it."); bp::def("rnea", &pinocchio_overload::rnea< double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd>, + bp::return_value_policy>(), (bp::arg("pinocchio_model"), "pinocchio_data", "q", "v", "a"), - "Compute the RNEA without external forces, store the result in Data and return it.", - bp::return_value_policy>()); + "Compute the RNEA without external forces, store the result in Data and return it."); bp::def("rnea", &pinocchio_overload::rnea< double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd, pinocchio::Force>, + bp::return_value_policy>(), (bp::arg("pinocchio_model"), "pinocchio_data", "q", "v", "a", "fext"), - "Compute the RNEA with external forces, store the result in Data and return it.", - bp::return_value_policy>()); + "Compute the RNEA with external forces, store the result in Data and return it."); bp::def("crba", &pinocchio_overload::crba< double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd>, + bp::return_value_policy>(), (bp::arg("pinocchio_model"), "pinocchio_data", "q", bp::arg("fast_math") = false), - "Computes CRBA, store the result in Data and return it.", - bp::return_value_policy>()); + "Computes CRBA, store the result in Data and return it."); bp::def("computeKineticEnergy", &pinocchio_overload::computeKineticEnergy< double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd>, @@ -343,6 +343,7 @@ namespace jiminy::python bp::def("computeJMinvJt", &pinocchio_overload::computeJMinvJt, + bp::return_value_policy>(), (bp::arg("pinocchio_model"), "pinocchio_data", "J", bp::arg("update_decomposition") = true)); bp::def("solveJMinvJtv", &solveJMinvJtv, (bp::arg("pinocchio_data"), "v", bp::arg("update_decomposition") = true)); diff --git a/python/jiminy_pywrap/src/module.cc b/python/jiminy_pywrap/src/module.cc index ad17f3ce9..f529fb46d 100755 --- a/python/jiminy_pywrap/src/module.cc +++ b/python/jiminy_pywrap/src/module.cc @@ -42,17 +42,6 @@ namespace jiminy::python (bp::arg("self"), "t", "q", "v")); } - template - void registerToPythonByValueConverter() - { - bp::type_info info = bp::type_id(); - const bp::converter::registration * reg = bp::converter::registry::query(info); - if (reg == nullptr || *reg->m_to_python == nullptr) - { - bp::to_python_converter, true>(); - } - } - BOOST_PYTHON_MODULE(PYTHON_LIBRARY_NAME) { /* Initialized boost::python::numpy. @@ -98,8 +87,8 @@ namespace jiminy::python bp::docstring_options doc_options; doc_options.disable_cpp_signatures(); - /* Enable some automatic C++ to Python converters. - By default, conversion is by-value unless specified explicitly via a call policy. */ + /* Enable some automatic C++ from/to Python converters. + By default, conversion is by-value unless overwritten via a call policy. */ registerToPythonByValueConverter(); // Expose functors @@ -129,9 +118,7 @@ namespace jiminy::python exposeFunctionalController(); exposeForces(); exposeStepperState(); - exposeSystemState(); - exposeSystem(); - exposeEngineMultiRobot(); + exposeRobotState(); exposeEngine(); } diff --git a/python/jiminy_pywrap/src/robot.cc b/python/jiminy_pywrap/src/robot.cc index f6493b5ae..1366365f7 100644 --- a/python/jiminy_pywrap/src/robot.cc +++ b/python/jiminy_pywrap/src/robot.cc @@ -3,6 +3,7 @@ #include "jiminy/core/hardware/abstract_sensor.h" #include "jiminy/core/hardware/abstract_motor.h" #include "jiminy/core/constraints/abstract_constraint.h" +#include "jiminy/core/control/abstract_controller.h" #include "jiminy/core/robot/robot.h" #include "pinocchio/bindings/python/fwd.hpp" @@ -98,9 +99,6 @@ namespace jiminy::python .ADD_PROPERTY_GET_WITH_POLICY("mesh_package_dirs", &Model::getMeshPackageDirs, bp::return_value_policy>()) - .ADD_PROPERTY_GET_WITH_POLICY("name", - &Model::getName, - bp::return_value_policy()) .ADD_PROPERTY_GET_WITH_POLICY("urdf_path", &Model::getUrdfPath, bp::return_value_policy()) @@ -283,7 +281,10 @@ namespace jiminy::python static void expose() { // clang-format off - bp::class_, boost::noncopyable>("Model", bp::no_init) + bp::class_, + boost::noncopyable + >("Model", bp::no_init) .def(PyModelVisitor()); // clang-format on } @@ -317,6 +318,10 @@ namespace jiminy::python &Robot::getIsLocked, bp::return_value_policy()) + .ADD_PROPERTY_GET_WITH_POLICY("name", + &Robot::getName, + bp::return_value_policy()) + .def("dump_options", &Robot::dumpOptions, (bp::arg("self"), "json_filename")) .def("load_options", &Robot::loadOptions, @@ -347,6 +352,12 @@ namespace jiminy::python >(&Robot::getSensor), (bp::arg("self"), "sensor_type", "sensor_name")) + .ADD_PROPERTY_GET_SET("controller", + static_cast< + std::shared_ptr (Robot::*)() + >(&Robot::getController), + &Robot::setController) + .ADD_PROPERTY_GET("sensor_measurements", &PyRobotVisitor::getSensorMeasurements) .def("set_options", &PyRobotVisitor::setOptions, @@ -360,10 +371,10 @@ namespace jiminy::python .def("get_motors_options", &Robot::getMotorsOptions) .def("set_sensors_options", &PyRobotVisitor::setSensorsOptions, (bp::arg("self"), "sensors_options")) - .def("get_sensors_options", - static_cast< - GenericConfig (Robot::*)(void) const - >(&Robot::getSensorsOptions)) + .def("get_sensors_options", &Robot::getSensorsOptions) + .def("set_controller_options", &PyRobotVisitor::setControllerOptions, + (bp::arg("self"), "controller_options")) + .def("get_controller_options", &Robot::getControllerOptions) .def("set_telemetry_options", &PyRobotVisitor::setTelemetryOptions, (bp::arg("self"), "telemetry_options")) .def("get_telemetry_options", &Robot::getTelemetryOptions) @@ -454,6 +465,13 @@ namespace jiminy::python return self.setSensorsOptions(config); } + static void setControllerOptions(Robot & self, const bp::dict & configPy) + { + GenericConfig config = self.getControllerOptions(); + convertFromPython(configPy, config); + return self.setControllerOptions(config); + } + static void setTelemetryOptions(Robot & self, const bp::dict & configPy) { GenericConfig config = self.getTelemetryOptions(); @@ -464,7 +482,10 @@ namespace jiminy::python static void expose() { // clang-format off - bp::class_, std::shared_ptr, boost::noncopyable>("Robot") + bp::class_, + std::shared_ptr, + boost::noncopyable + >("Robot", bp::init(bp::arg("name") = "")) .def(PyRobotVisitor()); // clang-format on }