duburcqa · duburcqa · Dec 28, 2021 · Dec 28, 2021 · Dec 28, 2021 · Dec 28, 2021
diff --git a/core/include/jiminy/core/engine/EngineMultiRobot.h b/core/include/jiminy/core/engine/EngineMultiRobot.h
@@ -392,10 +392,10 @@ namespace jiminy
         /// \param[in] forceFct Callback function returning the force that systemName2
         ///                     applies on systemName1, in the global frame of frameName1.
         hresult_t registerForceCoupling(std::string const & systemName1,
-                                   std::string const & systemName2,
-                                   std::string const & frameName1,
-                                   std::string const & frameName2,
-                                   forceCouplingFunctor_t forceFct);
+                                        std::string const & systemName2,
+                                        std::string const & frameName1,
+                                        std::string const & frameName2,
+                                        forceCouplingFunctor_t forceFct);
         hresult_t registerViscoElasticDirectionalForceCoupling(std::string const & systemName1,
                                                                std::string const & systemName2,
                                                                std::string const & frameName1,

diff --git a/core/include/jiminy/core/robot/Model.h b/core/include/jiminy/core/robot/Model.h
@@ -58,7 +58,7 @@ namespace jiminy
 
         std::shared_ptr<AbstractConstraintBase> get(std::string const & key);
         std::shared_ptr<AbstractConstraintBase> get(std::string const & key,
-                                                constraintsHolderType_t const & holderType);
+                                                    constraintsHolderType_t const & holderType);
 
         void insert(constraintsMap_t const & constraintsMap,
                     constraintsHolderType_t const & holderType);

diff --git a/core/include/jiminy/core/stepper/AbstractRungeKuttaStepper.h b/core/include/jiminy/core/stepper/AbstractRungeKuttaStepper.h
@@ -19,7 +19,7 @@ namespace jiminy
             /// \brief Constructor
             /// \param[in] f      Dynamics function, with signature a = f(t, q, v)
             /// \param[in] robots Robots whose dynamics the stepper will work on.
-            AbstractRungeKuttaStepper(systemDynamics f, /* Copy on purpose */
+            AbstractRungeKuttaStepper(systemDynamics const & f,
                                       std::vector<Robot const *> const & robots,
                                       matrixN_t const & RungeKuttaMatrix,
                                       vectorN_t const & bWeights,

diff --git a/core/include/jiminy/core/stepper/AbstractStepper.h b/core/include/jiminy/core/stepper/AbstractStepper.h
@@ -33,7 +33,7 @@ namespace jiminy
         /// \brief Constructor
         /// \param[in] f       Dynamics function, with signature a = f(t, q, v)
         /// \param[in] robots  Robots whose dynamics the stepper will work on.
-        AbstractStepper(systemDynamics f, /* Copy on purpose */
+        AbstractStepper(systemDynamics const & f,
                         std::vector<Robot const *> const & robots);
         virtual ~AbstractStepper(void) = default;
 

diff --git a/core/include/jiminy/core/stepper/EulerExplicitStepper.h b/core/include/jiminy/core/stepper/EulerExplicitStepper.h
@@ -19,7 +19,7 @@ namespace jiminy
             /// \brief Constructor
             /// \param[in] f      Dynamics function, with signature a = f(t, q, v)
             /// \param[in] robots Robots whose dynamics the stepper will work on.
-            EulerExplicitStepper(systemDynamics f, /* Copy on purpose */
+            EulerExplicitStepper(systemDynamics const & f,
                                  std::vector<Robot const *> const & robots);
 
         protected:

diff --git a/core/include/jiminy/core/stepper/RungeKutta4Stepper.h b/core/include/jiminy/core/stepper/RungeKutta4Stepper.h
@@ -27,7 +27,7 @@ namespace jiminy
             /// \brief Constructor
             /// \param[in] f      Dynamics function, with signature a = f(t, q, v)
             /// \param[in] robots Robots whose dynamics the stepper will work on.
-            RungeKutta4Stepper(systemDynamics f, /* Copy on purpose */
+            RungeKutta4Stepper(systemDynamics const & f,
                                std::vector<Robot const *> const & robots);
     };
 }

diff --git a/core/include/jiminy/core/stepper/RungeKuttaDOPRIStepper.h b/core/include/jiminy/core/stepper/RungeKuttaDOPRIStepper.h
@@ -57,7 +57,7 @@ namespace jiminy
             /// \param[in] robots Robots whose dynamics the stepper will work on.
             /// \param[in] tolRel Relative tolerance, used to determine step success and timestep update.
             /// \param[in] tolAbs Relative tolerance, used to determine step success and timestep update.
-            RungeKuttaDOPRIStepper(systemDynamics f, /* Copy on purpose */
+            RungeKuttaDOPRIStepper(systemDynamics const & f,
                                    std::vector<Robot const *> const & robots,
                                    float64_t const & tolRel,
                                    float64_t const & tolAbs);

diff --git a/core/include/jiminy/core/utilities/Pinocchio.h b/core/include/jiminy/core/utilities/Pinocchio.h
@@ -118,6 +118,13 @@ namespace jiminy
                                   pinocchio::GeometryModel & collisionModel,
                                   boost::optional<pinocchio::GeometryModel &> visualModel = boost::none,
                                   bool_t const & loadVisualMeshes = false);
+
+    void buildReducedModel(pinocchio::Model const & inputModel,
+                           pinocchio::GeometryModel const & inputGeomModel,
+                           std::vector<pinocchio::JointIndex> const & listOfJointsToLock,
+                           vectorN_t const & referenceConfiguration,
+                           pinocchio::Model & reducedModel,
+                           pinocchio::GeometryModel & reducedGeomModel);
 }
 
 #endif  // JIMINY_PINOCCHIO_H
diff --git a/core/src/control/AbstractController.cc b/core/src/control/AbstractController.cc
@@ -144,7 +144,7 @@ namespace jiminy
                 {
                     objectName = objectPrefixName + TELEMETRY_FIELDNAME_DELIMITER + objectName;
                 }
-                telemetrySender_.configureObject(std::move(telemetryData), objectName);
+                telemetrySender_.configureObject(telemetryData, objectName);
                 for (std::pair<std::string, float64_t const *> const & registeredVariable : registeredVariables_)
                 {
                     if (returnCode == hresult_t::SUCCESS)

diff --git a/core/src/engine/Engine.cc b/core/src/engine/Engine.cc
@@ -43,12 +43,12 @@ namespace jiminy
         if (controller)
         {
             returnCode = EngineMultiRobot::addSystem(
-                "", std::move(robot), std::move(controller), std::move(callbackFct));
+                "", robot, controller, std::move(callbackFct));
         }
         else
         {
             returnCode = EngineMultiRobot::addSystem(
-                "", std::move(robot), std::move(callbackFct));
+                "", robot, std::move(callbackFct));
         }
 
         if (returnCode == hresult_t::SUCCESS)

diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc
@@ -103,12 +103,24 @@ namespace jiminy
             return hresult_t::ERROR_GENERIC;
         }
 
+        if (!robot)
+        {
+            PRINT_ERROR("Robot unspecified.");
+            return hresult_t::ERROR_INIT_FAILED;
+        }
+
         if (!robot->getIsInitialized())
         {
             PRINT_ERROR("Robot not initialized.");
             return hresult_t::ERROR_INIT_FAILED;
         }
 
+        if (!controller)
+        {
+            PRINT_ERROR("Controller unspecified.");
+            return hresult_t::ERROR_INIT_FAILED;
+        }
+
         if (!controller->getIsInitialized())
         {
             PRINT_ERROR("Controller not initialized.");
@@ -130,8 +142,8 @@ namespace jiminy
 
         // TODO: Check that the callback function is working as expected
         systems_.emplace_back(systemName,
-                              std::move(robot),
-                              std::move(controller),
+                              robot,
+                              controller,
                               std::move(callbackFct));
         systemsDataHolder_.resize(systems_.size());
 
@@ -142,6 +154,12 @@ namespace jiminy
                                           std::shared_ptr<Robot> robot,
                                           callbackFunctor_t callbackFct)
     {
+        if (!robot)
+        {
+            PRINT_ERROR("Robot unspecified.");
+            return hresult_t::ERROR_INIT_FAILED;
+        }
+
         if (!robot->getIsInitialized())
         {
             PRINT_ERROR("Robot not initialized.");
@@ -318,13 +336,13 @@ namespace jiminy
         if (returnCode == hresult_t::SUCCESS)
         {
             forcesCoupling_.emplace_back(systemName1,
-                                         std::move(systemIdx1),
+                                         systemIdx1,
                                          systemName2,
-                                         std::move(systemIdx2),
+                                         systemIdx2,
                                          frameName1,
-                                         std::move(frameIdx1),
+                                         frameIdx1,
                                          frameName2,
-                                         std::move(frameIdx2),
+                                         frameIdx2,
                                          std::move(forceFct));
         }
 

diff --git a/core/src/engine/System.cc b/core/src/engine/System.cc
@@ -80,8 +80,8 @@ namespace jiminy
                                    std::shared_ptr<AbstractController> controllerIn,
                                    callbackFunctor_t callbackFctIn) :
     name(systemNameIn),
-    robot(std::move(robotIn)),
-    controller(std::move(controllerIn)),
+    robot(robotIn),
+    controller(controllerIn),
     callbackFct(std::move(callbackFctIn))
     {
         // Empty on purpose

diff --git a/core/src/io/JsonLoader.cc b/core/src/io/JsonLoader.cc
@@ -9,7 +9,7 @@ namespace jiminy
     JsonLoader::JsonLoader(std::shared_ptr<AbstractIODevice> device) :
     rootJson_(std::make_unique<Json::Value>()),
     payload_(),
-    device_(std::move(device))
+    device_(device)
     {
         // Empty on purpose.
     }

diff --git a/core/src/robot/AbstractSensor.cc b/core/src/robot/AbstractSensor.cc
@@ -42,7 +42,7 @@ namespace jiminy
                     {
                         objectName = objectPrefixName + TELEMETRY_FIELDNAME_DELIMITER + objectName;
                     }
-                    telemetrySender_.configureObject(std::move(telemetryData), objectName);
+                    telemetrySender_.configureObject(telemetryData, objectName);
                     returnCode = telemetrySender_.registerVariable(getFieldnames(), get());
                     if (returnCode == hresult_t::SUCCESS)
                     {

diff --git a/core/src/robot/Robot.cc b/core/src/robot/Robot.cc
@@ -105,7 +105,7 @@ namespace jiminy
 
         if (returnCode == hresult_t::SUCCESS)
         {
-            telemetryData_ = std::move(telemetryData);
+            telemetryData_ = telemetryData;
         }
 
         if (returnCode == hresult_t::SUCCESS)

diff --git a/core/src/stepper/AbstractRungeKuttaStepper.cc b/core/src/stepper/AbstractRungeKuttaStepper.cc
@@ -4,7 +4,7 @@
 
 namespace jiminy
 {
-    AbstractRungeKuttaStepper::AbstractRungeKuttaStepper(systemDynamics f, /* Copy on purpose */
+    AbstractRungeKuttaStepper::AbstractRungeKuttaStepper(systemDynamics const & f,
                                                          std::vector<Robot const *> const & robots,
                                                          matrixN_t const & RungeKuttaMatrix,
                                                          vectorN_t const & bWeights,

diff --git a/core/src/stepper/AbstractStepper.cc b/core/src/stepper/AbstractStepper.cc
@@ -3,9 +3,9 @@
 
 namespace jiminy
 {
-    AbstractStepper::AbstractStepper(systemDynamics f,
+    AbstractStepper::AbstractStepper(systemDynamics const & f,
                                      std::vector<Robot const *> const & robots):
-    f_(std::move(f)),
+    f_(f),
     robots_(robots),
     state_(robots),
     stateDerivative_(robots),

diff --git a/core/src/stepper/EulerExplicitStepper.cc b/core/src/stepper/EulerExplicitStepper.cc
@@ -3,7 +3,7 @@
 
 namespace jiminy
 {
-    EulerExplicitStepper::EulerExplicitStepper(systemDynamics f, /* Copy on purpose */
+    EulerExplicitStepper::EulerExplicitStepper(systemDynamics const & f,
                                                std::vector<Robot const *> const & robots):
     AbstractStepper(f, robots)
     {

diff --git a/core/src/stepper/RungeKutta4Stepper.cc b/core/src/stepper/RungeKutta4Stepper.cc
@@ -2,7 +2,7 @@
 
 namespace jiminy
 {
-    RungeKutta4Stepper::RungeKutta4Stepper(systemDynamics f, /* Copy on purpose */
+    RungeKutta4Stepper::RungeKutta4Stepper(systemDynamics const & f,
                                            std::vector<Robot const *> const & robots):
     AbstractRungeKuttaStepper(f, robots, RK4::A, RK4::b, RK4::c, false)
     {

diff --git a/core/src/stepper/RungeKuttaDOPRIStepper.cc b/core/src/stepper/RungeKuttaDOPRIStepper.cc
@@ -2,7 +2,7 @@
 
 namespace jiminy
 {
-    RungeKuttaDOPRIStepper::RungeKuttaDOPRIStepper(systemDynamics f, /* Copy on purpose */
+    RungeKuttaDOPRIStepper::RungeKuttaDOPRIStepper(systemDynamics const & f,
                                                    std::vector<Robot const *> const & robots,
                                                    float64_t const & tolRel,
                                                    float64_t const & tolAbs):

diff --git a/core/src/telemetry/TelemetrySender.cc b/core/src/telemetry/TelemetrySender.cc
@@ -98,7 +98,7 @@ namespace jiminy
                                           std::string const & objectNameIn)
     {
         objectName_ = objectNameIn;
-        telemetryData_ = std::move(telemetryDataInstance);
+        telemetryData_ = telemetryDataInstance;
         intBufferPosition_.clear();
         floatBufferPosition_.clear();
     }

diff --git a/core/src/utilities/Pinocchio.cc b/core/src/utilities/Pinocchio.cc
@@ -11,6 +11,7 @@
 #include "pinocchio/multibody/visitor.hpp"                 // `pinocchio::fusion::JointUnaryVisitorBase`
 #include "pinocchio/multibody/joint/joint-model-base.hpp"  // `pinocchio::JointModelBase`
 #include "pinocchio/algorithm/joint-configuration.hpp"     // `pinocchio::isNormalized`
+#include "pinocchio/algorithm/model.hpp"                   // `pinocchio::buildReducedModel`
 
 #include "hpp/fcl/mesh_loader/loader.h"
 #include "hpp/fcl/BVH/BVH_model.h"
@@ -275,7 +276,7 @@ namespace jiminy
             {
                 frameIndex_t frameIdx;
                 returnCode = getFrameIdx(model, name, frameIdx);
-                framesIdx.push_back(std::move(frameIdx));
+                framesIdx.push_back(frameIdx);
             }
         }
 
@@ -310,7 +311,7 @@ namespace jiminy
             {
                 frameIndex_t frameIdx;
                 returnCode = getFrameIdx(model, name, frameIdx);
-                bodiesIdx.push_back(std::move(frameIdx));
+                bodiesIdx.push_back(frameIdx);
             }
         }
 
@@ -1053,4 +1054,26 @@ namespace jiminy
 
         return returnCode;
     }
+
+    void buildReducedModel(pinocchio::Model const & inputModel,
+                           pinocchio::GeometryModel const & inputGeomModel,
+                           std::vector<pinocchio::JointIndex> const & listOfJointsToLock,
+                           vectorN_t const & referenceConfiguration,
+                           pinocchio::Model & reducedModel,
+                           pinocchio::GeometryModel & reducedGeomModel)
+    {
+        // Fix `parentFrame` not updated for reduced geometry model in Pinocchio < 2.6.0
+        pinocchio::buildReducedModel(inputModel,
+                                     inputGeomModel,
+                                     listOfJointsToLock,
+                                     referenceConfiguration,
+                                     reducedModel,
+                                     reducedGeomModel);
+        for (auto const & geom : inputGeomModel.geometryObjects)
+        {
+            geomIndex_t reducedGeomIdx = reducedGeomModel.getGeometryId(geom.name);
+            auto & reducedGeom = reducedGeomModel.geometryObjects[reducedGeomIdx];
+            reducedGeom.parentFrame = reducedModel.getBodyId(inputModel.frames[geom.parentFrame].name);
+        }
+    }
 }
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
@@ -11,7 +11,8 @@
                             ContactSensor as contact,
                             ForceSensor as force,
                             ImuSensor as imu,
-                            PeriodicGaussianProcess)
+                            PeriodicGaussianProcess,
+                            Robot)
 from jiminy_py.simulator import Simulator
 
 import pinocchio as pin
@@ -74,7 +75,7 @@ class WalkerJiminyEnv(BaseJiminyEnv):
     }
 
     def __init__(self,
-                 urdf_path: str,
+                 urdf_path: Optional[str],
                  hardware_path: Optional[str] = None,
                  mesh_path: Optional[str] = None,
                  simu_duration_max: float = DEFAULT_SIMULATION_DURATION,
@@ -84,7 +85,8 @@ def __init__(self,
                  std_ratio: Optional[dict] = None,
                  config_path: Optional[str] = None,
                  avoid_instable_collisions: bool = True,
-                 debug: bool = False,
+                 debug: bool = False, *,
+                 robot: Optional[Robot] = None,
                  **kwargs: Any) -> None:
         r"""
         :param urdf_path: Path of the urdf model to be used for the simulation.
@@ -118,6 +120,10 @@ def __init__(self,
                                           its vertices.
         :param debug: Whether or not the debug mode must be activated.
                       Doing it enables telemetry recording.
+        :param robot: Robot being simulated, already instantiated and
+                      initialized. Build default robot using 'urdf_path',
+                      'hardware_path' and 'mesh_path' if omitted.
+                      Optional: None by default.
         :param kwargs: Keyword arguments to forward to `BaseJiminyEnv` class.
         """
         # Handling of default arguments
@@ -155,15 +161,23 @@ def __init__(self,
         self._height_neutral = 0.0
 
         # Configure the backend simulator
-        simulator = Simulator.build(**{**dict(
-            urdf_path=self.urdf_path,
-            hardware_path=self.hardware_path,
-            mesh_path=self.mesh_path,
-            has_freeflyer=True,
-            use_theoretical_model=False,
-            config_path=self.config_path,
-            avoid_instable_collisions=self.avoid_instable_collisions,
-            debug=debug), **kwargs})
+        if robot is None:
+            assert isinstance(self.urdf_path, str)
+            simulator = Simulator.build(
+                urdf_path=self.urdf_path,
+                hardware_path=self.hardware_path,
+                mesh_path=self.mesh_path,
+                config_path=self.config_path,
+                avoid_instable_collisions=self.avoid_instable_collisions,
+                debug=debug,
+                **{**dict(
+                    has_freeflyer=True,
+                    use_theoretical_model=False),
+                    **kwargs})
+        else:
+            # Instantiate a simulator and load the options
+            simulator = Simulator(robot)
+            simulator.import_options(config_path)
 
         # Initialize base class
         super().__init__(