[core] Fix adding flexibility frame on root joint of fixed-base robot.

core/include/jiminy/core/engine/engine.h

@@ -326,9 +326,9 @@ namespace jiminy
             config["isPersistent"] = false;
             config["enableConfiguration"] = true;
             config["enableVelocity"] = true;
-            config["enableAcceleration"] = false;
+            config["enableAcceleration"] = true;
             config["enableForceExternal"] = false;
-            config["enableCommand"] = false;
+            config["enableCommand"] = true;
             config["enableEffort"] = false;
             config["enableEnergy"] = false;
             return config;

core/src/hardware/abstract_motor.cc

@@ -350,24 +350,16 @@ namespace jiminy
         }
 
         // Get the motor velocity limits on motor side from the URDF or the user options
-        if (baseMotorOptions_->enableVelocityLimit)
+        if (baseMotorOptions_->velocityLimitFromUrdf)
         {
-            if (baseMotorOptions_->velocityLimitFromUrdf)
-            {
-                const Eigen::Index mechanicalJointVelocityIndex =
-                    getJointVelocityFirstIndex(robot->pinocchioModelTh_, jointName_);
-                velocityLimit_ =
-                    robot->pinocchioModelTh_.velocityLimit[mechanicalJointVelocityIndex] *
-                    mechanicalReduction;
-            }
-            else
-            {
-                velocityLimit_ = baseMotorOptions_->velocityLimit;
-            }
+            const Eigen::Index mechanicalJointVelocityIndex =
+                getJointVelocityFirstIndex(robot->pinocchioModelTh_, jointName_);
+            velocityLimit_ = robot->pinocchioModelTh_.velocityLimit[mechanicalJointVelocityIndex] *
+                             mechanicalReduction;
         }
         else
         {
-            velocityLimit_ = INF;
+            velocityLimit_ = baseMotorOptions_->velocityLimit;
         }
 
         // Get the rotor inertia on joint side

core/src/hardware/basic_motors.cc

@@ -86,14 +86,17 @@ namespace jiminy
         /* Compute the motor effort, taking into account velocity and effort limits.
            It is the output of the motor on joint side, ie after the transmission. */
         double effortMin = -effortLimit_;
-        if (vMotor < -velocityLimit_)
-        {
-            effortMin = 0.0;
-        }
         double effortMax = effortLimit_;
-        if (vMotor > velocityLimit_)
+        if (motorOptions_->enableVelocityLimit)
         {
-            effortMax = 0.0;
+            if (vMotor < -velocityLimit_)
+            {
+                effortMin = 0.0;
+            }
+            if (vMotor > velocityLimit_)
+            {
+                effortMax = 0.0;
+            }
         }
         data() = motorOptions_->mechanicalReduction * std::clamp(command, effortMin, effortMax);
 

core/src/io/serialization.cc

@@ -32,6 +32,7 @@
 #include "jiminy/core/hardware/abstract_sensor.h"
 #include "jiminy/core/hardware/basic_sensors.h"
 #include "jiminy/core/robot/robot.h"
+#include "jiminy/core/utilities/pinocchio.h"
 
 #define BOOST_FILESYSTEM_VERSION 3
 #include <boost/filesystem/operations.hpp>  // `boost::filesystem::unique_path`
@@ -672,8 +673,28 @@ namespace boost::serialization
         // Backup mesh package lookup directories
         ar << make_nvp("mesh_package_dirs", model.getMeshPackageDirs());
 
+        /* Copy the theoretical model then remove extra collision frames.
+           Note that `removeCollisionBodies` is not called to avoid altering the robot. */
+        pinocchio::Model pinocchioModelTh = model.pinocchioModelTh_;
+        std::vector<std::string> collisionConstraintNames;
+        for (const std::string & name : model.getCollisionBodyNames())
+        {
+            for (const pinocchio::GeometryObject & geom : model.collisionModelTh_.geometryObjects)
+            {
+                if (model.pinocchioModel_.frames[geom.parentFrame].name == name &&
+                    model.getConstraints().exist(geom.name, ConstraintNodeType::COLLISION_BODIES))
+                {
+                    const pinocchio::FrameIndex frameIndex =
+                        getFrameIndex(pinocchioModelTh, geom.name);
+                    pinocchioModelTh.frames.erase(
+                        std::next(pinocchioModelTh.frames.begin(), frameIndex));
+                    pinocchioModelTh.nframes--;
+                }
+            }
+        }
+
         // Backup theoretical and extended simulation models
-        ar << make_nvp("pinocchio_model_th", model.pinocchioModelTh_);
+        ar << make_nvp("pinocchio_model_th", pinocchioModelTh);
         ar << make_nvp("pinocchio_model", model.pinocchioModel_);
 
         /* Backup the Pinocchio GeometryModel for collisions and visuals if requested.

core/src/utilities/pinocchio.cc

@@ -616,13 +616,22 @@ namespace jiminy
             pinocchio::FrameIndex childFrameIndex = i;
             do
             {
-                childFrameIndex = model.frames[childFrameIndex].previousFrame;
+                const pinocchio::FrameIndex previousFrameIndex =
+                    model.frames[childFrameIndex].previousFrame;
+                if (childFrameIndex > 1 && previousFrameIndex == childFrameIndex)
+                {
+                    JIMINY_THROW(std::runtime_error,
+                                 "There is something wrong with the model. Frame '",
+                                 model.frames[childFrameIndex].name,
+                                 "' is its own parent.");
+                }
+                childFrameIndex = previousFrameIndex;
                 if (childFrameIndex == frameIndex)
                 {
                     childFrameIndices.push_back(i);
                     break;
                 }
-            } while (childFrameIndex > 0 &&
+            } while (childFrameIndex > 1 &&
                      model.frames[childFrameIndex].type != pinocchio::FrameType::JOINT);
         }
 

data/bipedal_robots/cassie/cassie_hardware.toml

@@ -142,31 +142,31 @@ motor_name = "toe_joint_motor_right"
 # ============= Encoder sensors ===============
 
 [Sensor.EncoderSensor.hip_abduction_left]
-motor_name = "hip_abduction_left"
+motor_name = "hip_abduction_motor_left"
 
 [Sensor.EncoderSensor.hip_rotation_left]
-motor_name = "hip_rotation_left"
+motor_name = "hip_rotation_motor_left"
 
 [Sensor.EncoderSensor.hip_flexion_left]
-motor_name = "hip_flexion_left"
+motor_name = "hip_flexion_motor_left"
 
 [Sensor.EncoderSensor.knee_joint_left]
-motor_name = "knee_joint_left"
+motor_name = "knee_joint_motor_left"
 
 [Sensor.EncoderSensor.toe_joint_left]
-motor_name = "toe_joint_left"
+motor_name = "toe_joint_motor_left"
 
 [Sensor.EncoderSensor.hip_abduction_right]
-motor_name = "hip_abduction_right"
+motor_name = "hip_abduction_motor_right"
 
 [Sensor.EncoderSensor.hip_rotation_right]
-motor_name = "hip_rotation_right"
+motor_name = "hip_rotation_motor_right"
 
 [Sensor.EncoderSensor.hip_flexion_right]
-motor_name = "hip_flexion_right"
+motor_name = "hip_flexion_motor_right"
 
 [Sensor.EncoderSensor.knee_joint_right]
-motor_name = "knee_joint_right"
+motor_name = "knee_joint_motor_right"
 
 [Sensor.EncoderSensor.toe_joint_right]
-motor_name = "toe_joint_right"
+motor_name = "toe_joint_motor_right"

data/toys_models/ant/ant_hardware.toml

@@ -11,62 +11,62 @@ contactFrameNames = []  #['tip_1', 'tip_2', 'tip_3', 'tip_4']
 joint_name = "hip_1"
 enableVelocityLimit = false
 mechanicalReduction = 150.0
-armature = 0.00044
+armature = 4.4e-05
 frictionViscousPositive = -1.0
 frictionViscousNegative = -1.0
 
 [Motor.SimpleMotor.ankle_1]
 joint_name = "ankle_1"
 enableVelocityLimit = false
 mechanicalReduction = 150.0
-armature = 0.00044
+armature = 4.4e-05
 frictionViscousPositive = -1.0
 frictionViscousNegative = -1.0
 
 [Motor.SimpleMotor.hip_2]
 joint_name = "hip_2"
 enableVelocityLimit = false
 mechanicalReduction = 150.0
-armature = 0.00044
+armature = 4.4e-05
 frictionViscousPositive = -1.0
 frictionViscousNegative = -1.0
 
 [Motor.SimpleMotor.ankle_2]
 joint_name = "ankle_2"
 enableVelocityLimit = false
 mechanicalReduction = 150.0
-armature = 0.00044
+armature = 4.4e-05
 frictionViscousPositive = -1.0
 frictionViscousNegative = -1.0
 
 [Motor.SimpleMotor.hip_3]
 joint_name = "hip_3"
 enableVelocityLimit = false
 mechanicalReduction = 150.0
-armature = 0.00044
+armature = 4.4e-05
 frictionViscousPositive = -1.0
 frictionViscousNegative = -1.0
 
 [Motor.SimpleMotor.ankle_3]
 joint_name = "ankle_3"
 enableVelocityLimit = false
 mechanicalReduction = 150.0
-armature = 0.00044
+armature = 4.4e-05
 frictionViscousPositive = -1.0
 frictionViscousNegative = -1.0
 
 [Motor.SimpleMotor.hip_4]
 joint_name = "hip_4"
 enableVelocityLimit = false
 mechanicalReduction = 150.0
-armature = 0.00044
+armature = 4.4e-05
 frictionViscousPositive = -1.0
 frictionViscousNegative = -1.0
 
 [Motor.SimpleMotor.ankle_4]
 joint_name = "ankle_4"
 mechanicalReduction = 150.0
-armature = 0.00044
+armature = 4.4e-05
 frictionViscousPositive = -1.0
 frictionViscousNegative = -1.0
 

python/gym_jiminy/common/gym_jiminy/common/blocks/deformation_estimator.py

@@ -260,8 +260,10 @@ def get_flexibility_imu_frame_chains(
     supports = []
     for joint_index in leaf_joint_indices:
         support = []
-        while joint_index > 0:
+        while True:
             support.append(joint_index)
+            if joint_index == 0:
+                break
             joint_index = parents[joint_index]
         supports.append(support)
 
@@ -544,16 +546,17 @@ def __init__(self,
         # Backup some of the user-argument(s)
         self.ignore_twist = ignore_twist
 
-        # Define proxies for fast access
-        self.pinocchio_model_th = env.robot.pinocchio_model_th.copy()
-        self.pinocchio_data_th = env.robot.pinocchio_data_th.copy()
-
         # Create flexible dynamic model.
         # Dummy physical parameters are specified as they have no effect on
         # kinematic computations.
         model = jiminy.Model()
-        model.initialize(env.robot.pinocchio_model_th)
+        pinocchio_model_th = env.robot.pinocchio_model_th
+        if env.robot.has_freeflyer:
+            pinocchio_model_th = pin.buildReducedModel(
+                pinocchio_model_th, [1], pin.neutral(pinocchio_model_th))
+        model.initialize(pinocchio_model_th)
         options = model.get_options()
+        options["dynamics"]["enableFlexibility"] = True
         for frame_name in flex_frame_names:
             options["dynamics"]["flexibilityConfig"].append(
                 {
@@ -565,6 +568,10 @@ def __init__(self,
             )
         model.set_options(options)
 
+        # Backup theoretical pinocchio model without floating base
+        self.pinocchio_model_th = model.pinocchio_model_th.copy()
+        self.pinocchio_data_th = model.pinocchio_data_th.copy()
+
         # Extract contiguous chains of flexibility and IMU frames for which
         # computations can be vectorized. It also stores the information of
         # whether or not the sign of the deformation must be reversed to be
@@ -648,15 +655,16 @@ def __init__(self,
 
         # Extract mapping from encoders to theoretical configuration.
         # Note that revolute unbounded joints are not supported for now.
-        self.encoder_to_config_position_map = [-1,] * env.robot.nmotors
+        self.encoder_to_position_map = [-1,] * env.robot.nmotors
         for sensor in env.robot.sensors[EncoderSensor.type]:
             assert isinstance(sensor, EncoderSensor)
-            joint = self.pinocchio_model_th.joints[sensor.joint_index]
+            joint_index = self.pinocchio_model_th.getJointId(sensor.joint_name)
+            joint = self.pinocchio_model_th.joints[joint_index]
             joint_type = jiminy.get_joint_type(joint)
             if joint_type == jiminy.JointModelType.ROTARY_UNBOUNDED:
                 raise ValueError(
                     "Revolute unbounded joints are not supported for now.")
-            self.encoder_to_config_position_map[sensor.index] = joint.idx_q
+            self.encoder_to_position_map[sensor.index] = joint.idx_q
 
         # Extract measured motor / joint positions for fast access.
         # Note that they will be initialized in `_setup` method.
@@ -704,8 +712,12 @@ def _setup(self) -> None:
         # Refresh the theoretical model of the robot.
         # Even if the robot may change, the theoretical model of the robot is
         # not supposed to change in a way that would break this observer.
-        self.pinocchio_model_th = self.env.robot.pinocchio_model_th
-        self.pinocchio_data_th = self.env.robot.pinocchio_data_th
+        pinocchio_model_th = self.env.robot.pinocchio_model_th
+        if self.env.robot.has_freeflyer:
+            pinocchio_model_th = pin.buildReducedModel(
+                pinocchio_model_th, [1], pin.neutral(pinocchio_model_th))
+        self.pinocchio_model_th = pinocchio_model_th
+        self.pinocchio_data_th = self.pinocchio_model_th.createData()
 
         # Fix initialization of the observation to be valid quaternions
         self.observation[-1] = 1.0
@@ -754,9 +766,10 @@ def refresh_observation(self, measurement: BaseObsT) -> None:
         joint_positions = self.encoder_to_joint_ratio * self.encoder_data
 
         # Update the configuration of the theoretical model of the robot
-        self._q_th[self.encoder_to_config_position_map] = joint_positions
+        self._q_th[self.encoder_to_position_map] = joint_positions
 
-        # Update kinematic quantities according to the estimated configuration
+        # Update kinematic quantities according to the estimated configuration.
+        # FIXME: Compute frame placement only for relevant IMUs.
         pin.framesForwardKinematics(
             self.pinocchio_model_th, self.pinocchio_data_th, self._q_th)
 

python/gym_jiminy/common/gym_jiminy/common/blocks/proportional_derivative_controller.py

@@ -305,10 +305,8 @@ def __init__(self,
         :param target_velocity_limit: Restrict maximum motor target velocities
                                       wrt their hardware specifications.
                                       Optional: "inf" by default.
-        :param target_acceleration_limit:
-            Restrict maximum motor target accelerations wrt their hardware
-            specifications.
-            Optional: "inf" by default.
+        :param target_acceleration_limit: Maximum motor target acceleration.
+                                          Optional: "inf" by default.
         """
         # Make sure the action space of the environment has not been altered
         if env.action_space is not env.unwrapped.action_space:
@@ -360,12 +358,9 @@ def __init__(self,
             min(motor.velocity_limit, target_velocity_limit)
             for motor in env.robot.motors])
 
-        # Compute acceleration bounds allowing unrestricted bang-bang control
-        range_limit = 2 * motors_velocity_limit / env.step_dt
-        effort_limit = self.motors_effort_limit / (
-            self.kp * env.step_dt * np.maximum(env.step_dt / 2, self.kd))
-        acceleration_limit = np.minimum(
-            np.minimum(range_limit, effort_limit), target_acceleration_limit)
+        # Define acceleration bounds
+        acceleration_limit = np.full(
+            (env.robot.nmotors,), target_acceleration_limit)
 
         # Compute command state bounds
         self._command_state_lower = np.stack([motors_position_lower,