diff --git a/core/include/jiminy/core/Utilities.tpp b/core/include/jiminy/core/Utilities.tpp index 90b34414d..610510504 100644 --- a/core/include/jiminy/core/Utilities.tpp +++ b/core/include/jiminy/core/Utilities.tpp @@ -189,7 +189,7 @@ namespace jiminy void eraseVector(std::vector & vect1, std::vector const & vect2) { - vect1.erase(std::remove_if (vect1.begin(), vect1.end(), + vect1.erase(std::remove_if(vect1.begin(), vect1.end(), [&vect2](auto const & elem1) { auto vect2It = std::find(vect2.begin(), vect2.end(), elem1); diff --git a/core/include/jiminy/core/robot/Model.h b/core/include/jiminy/core/robot/Model.h index c221db6e8..9bf68788c 100644 --- a/core/include/jiminy/core/robot/Model.h +++ b/core/include/jiminy/core/robot/Model.h @@ -153,11 +153,11 @@ namespace jiminy pinocchio::SE3 const & framePlacement); hresult_t removeFrame(std::string const & frameName); hresult_t addCollisionBodies(std::vector const & bodyNames); - hresult_t removeCollisionBodies(std::vector const & frameNames = {}); + hresult_t removeCollisionBodies(std::vector frameNames = {}); // Make a copy hresult_t addContactPoints(std::vector const & frameNames); hresult_t removeContactPoints(std::vector const & frameNames = {}); - hresult_t setOptions(configHolder_t modelOptions); // Make a copy ! + hresult_t setOptions(configHolder_t modelOptions); // Make a copy configHolder_t getOptions(void) const; /// This method are not intended to be called manually. The Engine is taking care of it. @@ -175,6 +175,7 @@ namespace jiminy std::vector const & getCollisionBodiesNames(void) const; std::vector const & getContactFramesNames(void) const; std::vector const & getCollisionBodiesIdx(void) const; + std::vector > const & getCollisionPairsIdx(void) const; std::vector const & getContactFramesIdx(void) const; std::vector const & getRigidJointsNames(void) const; std::vector const & getRigidJointsModelIdx(void) const; @@ -223,24 +224,25 @@ namespace jiminy bool_t hasFreeflyer_; configHolder_t mdlOptionsHolder_; - std::vector collisionBodiesNames_; ///< Name of the collision bodies of the robot - std::vector contactFramesNames_; ///< Name of the contact frames of the robot - std::vector collisionBodiesIdx_; ///< Indices of the collision bodies in the frame list of the robot - std::vector contactFramesIdx_; ///< Indices of the contact frames in the frame list of the robot - std::vector rigidJointsNames_; ///< Name of the actual joints of the robot, not taking into account the freeflyer - std::vector rigidJointsModelIdx_; ///< Index of the actual joints in the pinocchio robot - std::vector rigidJointsPositionIdx_; ///< All the indices of the actual joints in the configuration vector of the robot (ie including all the degrees of freedom) - std::vector rigidJointsVelocityIdx_; ///< All the indices of the actual joints in the velocity vector of the robot (ie including all the degrees of freedom) - std::vector flexibleJointsNames_; ///< Name of the flexibility joints of the robot regardless of whether the flexibilities are enable - std::vector flexibleJointsModelIdx_; ///< Index of the flexibility joints in the pinocchio robot regardless of whether the flexibilities are enable - - vectorN_t positionLimitMin_; ///< Upper position limit of the whole configuration vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any) - vectorN_t positionLimitMax_; ///< Lower position limit of the whole configuration vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any) - vectorN_t velocityLimit_; ///< Maximum absolute velocity of the whole velocity vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any) - - std::vector positionFieldnames_; ///< Fieldnames of the elements in the configuration vector of the rigid robot - std::vector velocityFieldnames_; ///< Fieldnames of the elements in the velocity vector of the rigid robot - std::vector accelerationFieldnames_; ///< Fieldnames of the elements in the acceleration vector of the rigid robot + std::vector collisionBodiesNames_; ///< Name of the collision bodies of the robot + std::vector contactFramesNames_; ///< Name of the contact frames of the robot + std::vector collisionBodiesIdx_; ///< Indices of the collision bodies in the frame list of the robot + std::vector > collisionPairsIdx_; ///< Indices of the collision pairs associated with each collision body + std::vector contactFramesIdx_; ///< Indices of the contact frames in the frame list of the robot + std::vector rigidJointsNames_; ///< Name of the actual joints of the robot, not taking into account the freeflyer + std::vector rigidJointsModelIdx_; ///< Index of the actual joints in the pinocchio robot + std::vector rigidJointsPositionIdx_; ///< All the indices of the actual joints in the configuration vector of the robot (ie including all the degrees of freedom) + std::vector rigidJointsVelocityIdx_; ///< All the indices of the actual joints in the velocity vector of the robot (ie including all the degrees of freedom) + std::vector flexibleJointsNames_; ///< Name of the flexibility joints of the robot regardless of whether the flexibilities are enable + std::vector flexibleJointsModelIdx_; ///< Index of the flexibility joints in the pinocchio robot regardless of whether the flexibilities are enable + + vectorN_t positionLimitMin_; ///< Upper position limit of the whole configuration vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any) + vectorN_t positionLimitMax_; ///< Lower position limit of the whole configuration vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any) + vectorN_t velocityLimit_; ///< Maximum absolute velocity of the whole velocity vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any) + + std::vector positionFieldnames_; ///< Fieldnames of the elements in the configuration vector of the rigid robot + std::vector velocityFieldnames_; ///< Fieldnames of the elements in the velocity vector of the rigid robot + std::vector accelerationFieldnames_; ///< Fieldnames of the elements in the acceleration vector of the rigid robot private: pinocchio::Model pncModelFlexibleOrig_; diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc index 0d934e255..f5e85df9a 100644 --- a/core/src/engine/EngineMultiRobot.cc +++ b/core/src/engine/EngineMultiRobot.cc @@ -528,7 +528,7 @@ namespace jiminy auto xInitIt = xInit.find(system.name); if (xInitIt == xInit.end()) { - std::cout << "Error - EngineMultiRobot::start - At least one system does not have an initial state." << std::endl; + std::cout << "Error - EngineMultiRobot::start - At least one of the systems does not have an initial state." << std::endl; returnCode = hresult_t::ERROR_BAD_INPUT; } if (returnCode == hresult_t::SUCCESS) @@ -536,7 +536,22 @@ namespace jiminy if (xInitIt->second.rows() != system.robot->nx()) { std::cout << "Error - EngineMultiRobot::start - The size of the initial state is inconsistent " - "with model size for at least one system." << std::endl; + "with model size for at least one of the systems." << std::endl; + returnCode = hresult_t::ERROR_BAD_INPUT; + } + } + if (returnCode == hresult_t::SUCCESS) + { + auto const & qInit = xInitIt->second.head(system.robot->nq()).array(); + auto const & vInit = xInitIt->second.tail(system.robot->nv()).array(); + + // Note that EPS allows to be very slightly out-of-bounds. + if ((EPS < qInit - system.robot->getPositionLimitMax().array()).any() || + (EPS < system.robot->getPositionLimitMin().array() - qInit).any() || + (EPS < vInit.abs() - system.robot->getVelocityLimit().array()).any()) + { + std::cout << "Error - EngineMultiRobot::start - The initial state is out of bounds " + "for at least one of the systems." << std::endl; returnCode = hresult_t::ERROR_BAD_INPUT; } } @@ -691,11 +706,16 @@ namespace jiminy forceMax = std::max(forceMax, fext.linear().norm()); } - auto const & collisionBodiesIdx = system.robot->getCollisionBodiesIdx(); + std::vector const & collisionBodiesIdx = system.robot->getCollisionBodiesIdx(); + std::vector > const & collisionPairsIdx = system.robot->getCollisionPairsIdx(); for (uint32_t i=0; i < collisionBodiesIdx.size(); i++) { - pinocchio::Force fext = computeContactDynamicsAtBody(system, i); - forceMax = std::max(forceMax, fext.linear().norm()); + for (uint32_t j=0; j < collisionPairsIdx[i].size(); j++) + { + int32_t const & collisionPairIdx = collisionPairsIdx[i][j]; + pinocchio::Force fext = computeContactDynamicsAtBody(system, collisionPairIdx); + forceMax = std::max(forceMax, fext.linear().norm()); + } } if (forceMax > 1e5) @@ -893,6 +913,8 @@ namespace jiminy hresult_t EngineMultiRobot::step(float64_t stepSize) { + hresult_t returnCode = hresult_t::SUCCESS; + // Check if the simulation has started if (!isSimulationRunning_) { @@ -1011,7 +1033,7 @@ namespace jiminy timer_.tic(); // Perform the integration. Do not simulate extremely small time steps. - while (tEnd - t > STEPPER_MIN_TIMESTEP) + while ((tEnd - t > STEPPER_MIN_TIMESTEP) && (returnCode == hresult_t::SUCCESS)) { float64_t tNext = t; @@ -1357,14 +1379,14 @@ namespace jiminy { std::cout << "Error - EngineMultiRobot::step - Too many successive iteration failures. "\ "Probably something is going wrong with the physics. Aborting integration." << std::endl; - return hresult_t::ERROR_GENERIC; + returnCode = hresult_t::ERROR_GENERIC; } if (dt < STEPPER_MIN_TIMESTEP) { std::cout << "Error - EngineMultiRobot::step - The internal time step is getting too small. "\ "Impossible to integrate physics further in time." << std::endl; - return hresult_t::ERROR_GENERIC; + returnCode = hresult_t::ERROR_GENERIC; } timer_.toc(); @@ -1372,7 +1394,7 @@ namespace jiminy && engineOptions_->stepper.timeout < timer_.dt) { std::cout << "Error - EngineMultiRobot::step - Step computation timeout." << std::endl; - return hresult_t::ERROR_GENERIC; + returnCode = hresult_t::ERROR_GENERIC; } } @@ -1380,8 +1402,11 @@ namespace jiminy 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. */ - stepperState_.t = tEnd; - stepperState_.dt = stepSize; + if (returnCode == hresult_t::SUCCESS) + { + stepperState_.t = tEnd; + stepperState_.dt = stepSize; + } /* Monitor current iteration number, and log the current time, state, command, and sensors data. */ @@ -1390,7 +1415,12 @@ namespace jiminy updateTelemetry(); } - return hresult_t::SUCCESS; + if (returnCode != hresult_t::SUCCESS) + { + stop(); + } + + return returnCode; } void EngineMultiRobot::stop(void) @@ -1814,6 +1844,7 @@ namespace jiminy { pinocchio::forwardKinematics(system.robot->pncModel_, system.robot->pncData_, q, v, a); pinocchio::updateFramePlacements(system.robot->pncModel_, system.robot->pncData_); + pinocchio::centerOfMass(system.robot->pncModel_, system.robot->pncData_); pinocchio::updateGeometryPlacements(system.robot->pncModel_, system.robot->pncData_, system.robot->pncGeometryModel_, @@ -2132,16 +2163,21 @@ namespace jiminy // Compute the force at collision bodies std::vector const & collisionBodiesIdx = system.robot->getCollisionBodiesIdx(); + std::vector > const & collisionPairsIdx = system.robot->getCollisionPairsIdx(); for (uint32_t i=0; i < collisionBodiesIdx.size(); i++) { // Compute force at the given collision body. // It returns the force applied at the origin of the parent joint frame, in global frame int32_t const & frameIdx = collisionBodiesIdx[i]; - pinocchio::Force const fextLocal = computeContactDynamicsAtBody(system, i); - - // Apply the force at the origin of the parent joint frame, in local joint frame int32_t const & parentJointIdx = system.robot->pncModel_.frames[frameIdx].parent; - fext[parentJointIdx] += fextLocal; + for (uint32_t j=0; j < collisionPairsIdx[i].size(); j++) + { + int32_t const & collisionPairIdx = collisionPairsIdx[i][j]; + pinocchio::Force const fextLocal = computeContactDynamicsAtBody(system, collisionPairIdx); + + // Apply the force at the origin of the parent joint frame, in local joint frame + fext[parentJointIdx] += fextLocal; + } } // Add the effect of user-defined external impulse forces diff --git a/core/src/robot/Model.cc b/core/src/robot/Model.cc index bf4d4c2f8..0e637cd6d 100644 --- a/core/src/robot/Model.cc +++ b/core/src/robot/Model.cc @@ -34,6 +34,7 @@ namespace jiminy collisionBodiesNames_(), contactFramesNames_(), collisionBodiesIdx_(), + collisionPairsIdx_(), contactFramesIdx_(), rigidJointsNames_(), rigidJointsModelIdx_(), @@ -270,15 +271,15 @@ namespace jiminy pncGeometryModel_.addCollisionPair(collisionPair); } } - - // Refresh proxies associated with the collisions only - refreshCollisionsProxies(); } + // Refresh proxies associated with the collisions only + refreshCollisionsProxies(); + return hresult_t::SUCCESS; } - hresult_t Model::removeCollisionBodies(std::vector const & bodyNames) + hresult_t Model::removeCollisionBodies(std::vector bodyNames) { if (!isInitialized_) { @@ -300,14 +301,25 @@ namespace jiminy return hresult_t::ERROR_BAD_INPUT; } - // Remove the list of bodies from the set of collision bodies - if (!bodyNames.empty()) + /* Remove the list of bodies from the set of collision bodies, then + remove the associated set of collision pairs for each of them. */ + if (bodyNames.empty()) { - eraseVector(collisionBodiesNames_, bodyNames); + bodyNames = collisionBodiesNames_; } - else + + for (uint32_t i=0; icollisionRequest.num_max_contacts = mdlOptions_->collisions.maxContactPointsPerBody; #endif + // Extract the indices of the collision pairs associated with each body + collisionPairsIdx_.clear(); + for (std::string const & name : collisionBodiesNames_) + { + std::vector collisionPairsIdx; + for (uint32_t i=0; i > const & Model::getCollisionPairsIdx(void) const + { + return collisionPairsIdx_; + } + std::vector const & Model::getContactFramesIdx(void) const { return contactFramesIdx_; diff --git a/gym_jiminy/gym_jiminy/common/env_bases.py b/gym_jiminy/gym_jiminy/common/env_bases.py index df844bb7e..08cd6489a 100644 --- a/gym_jiminy/gym_jiminy/common/env_bases.py +++ b/gym_jiminy/gym_jiminy/common/env_bases.py @@ -52,6 +52,9 @@ class BaseJiminyEnv(gym.core.Env): It creates an Gym environment wrapping Jiminy Engine and behaves like any other Gym environment. """ + metadata = { + 'render.modes': ['human', 'rgb_array'], + } def __init__(self, engine_py: Optional[BaseJiminyEngine], @@ -80,13 +83,6 @@ def __init__(self, self._log_data = None self._log_file = None - ## Set the metadata of the environment. Those information are used by - # some gym wrappers such as VideoRecorder. - self.metadata = { - 'render.modes': ['human', 'rgb_array'], - 'video.frames_per_second': int(np.round(1.0 / self.dt)) - } - ## Use instance-specific action and observation spaces instead of the # class-wide ones provided by `gym.core.Env`. self.action_space = None @@ -113,10 +109,6 @@ def __init__(self, def robot(self) -> jiminy.Robot: return self.engine_py.robot - @property - def engine(self) -> jiminy.Engine: - return self.engine_py._engine - @property def log_path(self) -> Optional[str]: if self.debug is not None: @@ -368,7 +360,7 @@ def _sample_state(self) -> Tuple[np.ndarray, np.ndarray]: """ qpos = neutral(self.robot.pinocchio_model) if self.robot.has_freeflyer: - ground_fun = self.engine.get_options()['world']['groundProfile'] + ground_fun = self.engine_py.get_options()['world']['groundProfile'] compute_freeflyer_state_from_fixed_body( self.robot, qpos, ground_profile=ground_fun, use_theoretical_model=False) @@ -523,54 +515,57 @@ def step(self, action: np.ndarray (done or not), and a dictionary of extra information """ # Try to perform a single simulation step + is_step_failed = True try: self.engine_py.step(action_next=action, dt_desired=self.dt) + is_step_failed = False except RuntimeError as e: logger.error("Unrecoverable Jiminy engine exception:\n" + str(e)) - self._update_obs(self._observation) - return self._get_obs(), 0.0, True, {'is_success': False} self._update_obs(self._observation) - # Check if the simulation is over and if not already the case - done = self._is_done() - if done: - if self._steps_beyond_done is None: + # Check if the simulation is over + done = is_step_failed or self._is_done() + self._info = {'is_success': done} + + # Check if stepping after done and if it is an undefined behavior + if self._steps_beyond_done is None: + if done: self._steps_beyond_done = 0 - else: - if self._steps_beyond_done == 0: - logger.warn( - "Calling 'step' even though this environment has " - "already returned done = True whereas debug mode or " - "terminal reward is enabled. You must call 'reset' " - "to avoid further undefined behavior.") - self._steps_beyond_done += 1 + else: + if self._enable_reward_terminal and self._steps_beyond_done == 0: + logger.error( + "Calling 'step' even though this environment has " + "already returned done = True whereas terminal " + "reward is enabled. You must call 'reset' " + "to avoid further undefined behavior.") + self._steps_beyond_done += 1 + + # Early return in case of low-level engine integration failure + if is_step_failed: + return self._get_obs(), 0.0, done, self._info # Compute reward and extra information - self._info = {'is_success': done} reward, reward_info = self._compute_reward() if reward_info is not None: self._info['reward'] = reward_info # Finalize the episode is the simulation is over - if done: - if self._steps_beyond_done is None: - # Write log file if simulation is over (debug mode only) - if self.debug: - self.engine.write_log(self.log_path) - - if self._steps_beyond_done == 0 and \ - self._enable_reward_terminal: - # Extract log data from the simulation, which could be used - # for computing terminal reward. - self._log_data, _ = self.engine.get_log() - - # Compute the terminal reward - reward_final, reward_final_info = \ - self._compute_reward_terminal() - reward += reward_final - if reward_final_info is not None: - self._info.setdefault('reward', {}).update( - reward_final_info) + if done and self._steps_beyond_done == 0: + # Write log file if simulation is over (debug mode only) + if self.debug: + self.engine_py.write_log(self.log_path) + + # Extract log data from the simulation, which could be used + # for computing terminal reward. + self._log_data, _ = self.engine_py.get_log() + + # Compute the terminal reward + reward_final, reward_final_info = \ + self._compute_reward_terminal() + reward += reward_final + if reward_final_info is not None: + self._info.setdefault('reward', {}).update( + reward_final_info) return self._get_obs(), reward, done, self._info @@ -603,7 +598,7 @@ def replay(self, **kwargs) -> None: if self._log_data is not None: log_data = self._log_data else: - log_data, _ = self.engine.get_log() + log_data, _ = self.engine_py.get_log() self.engine_py._viewer = play_logfiles( [self.robot], [log_data], viewers=[self.engine_py._viewer], close_backend=False, verbose=True, **kwargs diff --git a/gym_jiminy/gym_jiminy/common/env_locomotion.py b/gym_jiminy/gym_jiminy/common/env_locomotion.py index cbffe3264..f61f6f7ee 100644 --- a/gym_jiminy/gym_jiminy/common/env_locomotion.py +++ b/gym_jiminy/gym_jiminy/common/env_locomotion.py @@ -4,6 +4,8 @@ import numba as nb from typing import Optional, Union +import gym + from jiminy_py.core import (EncoderSensor as encoder, EffortSensor as effort, ContactSensor as contact, @@ -148,7 +150,7 @@ def _setup_environment(self): # Remove already register forces self._forces_impulse = [] self._forces_profile = [] - self.engine.remove_forces() + self.engine_py.remove_forces() # Update some internal buffers used for computing the reward motor_effort_limit = self.robot.effort_limit[ @@ -161,16 +163,14 @@ def _setup_environment(self): # Compute the height of the freeflyer in neutral configuration # TODO: Take into account the ground profile. if self.robot.has_freeflyer: - q0 = neutral(self.robot.pinocchio_model) - compute_freeflyer_state_from_fixed_body(self.robot, q0, - ground_profile=None, use_theoretical_model=False) + q0, _ = self._sample_state() self._height_neutral = q0[2] else: self._height_neutral = None # Override some options of the robot and engine robot_options = self.robot.get_options() - engine_options = self.engine.get_options() + engine_options = self.engine_py.get_options() ### Make sure to log at least required data for reward # computation and log replay @@ -237,11 +237,12 @@ def _setup_environment(self): F_XY_IMPULSE_SCALE * self.rg.uniform() F = np.concatenate((F_xy_impulse, np.zeros(4))) t = t_ref + self.rg.uniform(low=-1.0, high=1.0) * 250e-3 - self._forces_impulse.append({ + force_impulse = { 'frame_name': frame_name, 't': t, 'dt': 10e-3, 'F': F - }) - self.engine.register_force_impulse(**self._forces_impulse[-1]) + } + self.engine_py.register_force_impulse(**forces_impulse) + self._forces_impulse.append(force_impulse) # Schedule a single force profile applied on PelvisLink. # Internally, it relies on a linear interpolation instead @@ -266,15 +267,16 @@ def F_xy_profile_interp1d(t): ratio * F_xy_profile[t_ind + 1] F_xy_profile_interp1d(0) # Pre-compilation self.F_xy_profile_spline = F_xy_profile_interp1d - self._forces_profile.append({ + force_profile.append({ 'frame_name': 'PelvisLink', 'force_function': self._force_external_profile }) - self.engine.register_force_profile(**self._forces_profile[-1]) + self.engine_py.register_force_profile(**force_profile) + self._forces_profile.append(force_profile) ### Set the options, finally self.robot.set_options(robot_options) - self.engine.set_options(engine_options) + self.engine_py.set_options(engine_options) def _force_external_profile(self, t, q, v, F): """ @@ -354,29 +356,6 @@ def _compute_reward_terminal(self): return reward_total, reward_dict - def step(self, action): - """ - @brief TODO - """ - # Perform a single simulation step - try: - obs, reward, done, info = super().step(action) - except RuntimeError: - obs = self.observation - reward = 0.0 - done = True - info = {'is_success': False} - - if done: - # Extract the log data from the simulation - self._log_data, _ = self.engine.get_log() - - # Write log file if simulation is over (debug mode only) - if self.debug and self._steps_beyond_done == 0: - self.engine.write_log(self.log_path) - - return obs, reward, done, info - class WalkerPDControlJiminyEnv(WalkerJiminyEnv): def __init__(self, @@ -391,12 +370,24 @@ def __init__(self, reward_mixture: Optional[dict] = None, std_ratio: Optional[dict] = None, debug: bool = False): + """ + @brief TODO + + @param hlc_to_llc_ratio High-level to Low-level control frequency + ratio. More precisely, at each step, the + command torque is updated 'hlc_to_llc_ratio' + times while the target motor state is only + updated once. + @param pid_kp PD controller position-proportional gain in motor order. + @param pid_kd PD controller velocity-proportional gain in motor order. + """ # Backup some user arguments self.hlc_to_llc_ratio = hlc_to_llc_ratio self.pid_kp = pid_kp self.pid_kd = pid_kd # Low-level controller buffers + self.motor_to_encoder = None self._q_target = None self._v_target = None @@ -404,29 +395,81 @@ def __init__(self, super().__init__(urdf_path, toml_path, mesh_path, simu_duration_max, dt, reward_mixture, std_ratio, debug) + def _setup_environment(self): + """ + @brief TODO + """ + # Setup the environment as usual + super()._setup_environment() + + # Refresh the mapping between the motors and encoders + encoder_joints = [] + for name in self.robot.sensors_names[encoder.type]: + sensor = self.robot.get_sensor(encoder.type, name) + encoder_joints.append(sensor.joint_name) + + self.motor_to_encoder = [] + for name in self.robot.motors_names: + motor = self.robot.get_motor(name) + motor_joint = motor.joint_name + encoder_found = False + for i, encoder_joint in enumerate(encoder_joints): + if motor_joint == encoder_joint: + self.motor_to_encoder.append(i) + encoder_found = True + break + if not encoder_found: + raise RuntimeError( + "No encoder sensor associated with motor '{name}'. Every " + "actuated joint must have an encoder sensor attached.") + def _refresh_action_space(self): """ @brief TODO """ - self.action_space = flatten_observation( - self.observation_space['sensors']['EncoderSensor']) + # Extract the position and velocity bounds for the observation space + encoder_space = self.observation_space['sensors'][encoder.type] + pos_high, vel_high = encoder_space.high + pos_low, vel_low = encoder_space.low + + # Reorder the position and velocity bounds to match motors order + pos_high[self.motor_to_encoder] = pos_high + vel_high[self.motor_to_encoder] = pos_high + pos_low[self.motor_to_encoder] = pos_low + vel_low[self.motor_to_encoder] = vel_low + + # Set the action space. Note that it is flattened. + self.action_space = gym.spaces.Box( + low=np.concatenate((pos_low, vel_low)), + high=np.concatenate((pos_high, vel_high)), + dtype=np.float64) def _compute_command(self): """ @brief TODO """ - # Extract estimated motor state based on sensors data + # Extract encoder data from the sensors data q_enc, v_enc = self.engine_py.sensors_data[encoder.type] + # Reorder the encoder data to match motors order + q_enc = q_enc[self.motor_to_encoder] + v_enc = v_enc[self.motor_to_encoder] + + # Compute the joint tracking error + q_err = q_enc - self._q_target + v_err = v_enc - self._v_target + # Compute PD command - u = - self.pid_kp * ((q_enc - self._q_target) + \ - self.pid_kd * (v_enc - self._v_target)) + u = - self.pid_kp * (q_err + self.pid_kd * v_err) return u def step(self, action): """ @brief TODO + + @params action Flattened array gathering the target motors positions + and velocities in this order. """ # Update target motor state self._q_target, self._v_target = np.split(action, 2, axis=-1) @@ -437,7 +480,7 @@ def step(self, action): for _ in range(self.hlc_to_llc_ratio): action = self._compute_command() obs, reward_step, done, info_step = super().step(action) - for k, v in info_step['reward'].items(): + for k, v in info_step.get('reward', {}).items(): reward_info[k].append(v) reward += reward_step if done: diff --git a/python/jiminy_py/src/jiminy_py/engine.py b/python/jiminy_py/src/jiminy_py/engine.py index a09c65d22..319e933b0 100644 --- a/python/jiminy_py/src/jiminy_py/engine.py +++ b/python/jiminy_py/src/jiminy_py/engine.py @@ -79,7 +79,7 @@ def __init__(self, # Real time rendering management self.step_dt_prev = -1 - self.is_ready = False + self.is_ready_to_start = False # Reset the low-level jiminy engine self._engine.reset() @@ -196,7 +196,7 @@ def reset(self, # Initialize some internal buffers self._t = 0.0 self.step_dt_prev = -1 - self.is_ready = True + self.is_ready_to_start = True # Stop the engine, to avoid locking the robot and the telemetry # too early, so that it remains possible to register external @@ -228,12 +228,13 @@ def step(self, @return Final state of the simulation """ if not self._engine.is_simulation_running: - if not self.is_ready: + if not self.is_ready_to_start: raise RuntimeError("Simulation not initialized. " "Please call 'reset' once before calling 'step'.") hresult = self._engine.start(self._state, self.use_theoretical_model) if (hresult != jiminy.hresult_t.SUCCESS): raise RuntimeError("Failed to start the simulation.") + self.is_ready_to_start = False if (action_next is not None): self.action = action_next diff --git a/python/jiminy_pywrap/include/jiminy/python/Jiminy.h b/python/jiminy_pywrap/include/jiminy/python/Jiminy.h index 6670e9a3e..46e8cda67 100644 --- a/python/jiminy_pywrap/include/jiminy/python/Jiminy.h +++ b/python/jiminy_pywrap/include/jiminy/python/Jiminy.h @@ -1037,6 +1037,10 @@ namespace python .add_property("collision_bodies_names", bp::make_function(&Model::getCollisionBodiesNames, bp::return_value_policy())) + .add_property("collision_bodies_idx", bp::make_function(&Model::getCollisionBodiesIdx, + bp::return_value_policy())) + .add_property("collision_pairs_idx_by_body", bp::make_function(&Model::getCollisionPairsIdx, + bp::return_value_policy())) .add_property("contact_frames_names", bp::make_function(&Model::getContactFramesNames, bp::return_value_policy())) .add_property("contact_frames_idx", bp::make_function(&Model::getContactFramesIdx, diff --git a/python/jiminy_pywrap/src/Module.cc b/python/jiminy_pywrap/src/Module.cc index 653761063..0f8874e4b 100755 --- a/python/jiminy_pywrap/src/Module.cc +++ b/python/jiminy_pywrap/src/Module.cc @@ -90,10 +90,11 @@ namespace python // Enable some automatic C++ to Python converters bp::to_python_converter, converterToPython > >(); - bp::to_python_converter, converterToPython > >(); - bp::to_python_converter, converterToPython > >(); - bp::to_python_converter, converterToPython > >(); - bp::to_python_converter >(); + bp::to_python_converter >, converterToPython > > >(); + bp::to_python_converter, converterToPython > >(); + bp::to_python_converter, converterToPython > >(); + bp::to_python_converter, converterToPython > >(); + bp::to_python_converter >(); // Expose classes TIME_STATE_FCT_EXPOSE(bool_t) diff --git a/python/jiminy_pywrap/src/python/__init__.py b/python/jiminy_pywrap/src/python/__init__.py index adff7cbf4..1637c6623 100755 --- a/python/jiminy_pywrap/src/python/__init__.py +++ b/python/jiminy_pywrap/src/python/__init__.py @@ -2,17 +2,9 @@ ## @brief Entry point for jiminy_pywrap python module. ############################################################################### -import os as _os # private import -import sys as _sys +import os as _os # private import -if (_sys.version_info > (3, 0)): - from contextlib import redirect_stderr as _redirect_stderr - with open(_os.devnull, 'w') as stderr, _redirect_stderr(stderr): - from .jiminy_pywrap import * - from .jiminy_pywrap import __version__, __raw_version__ -else: - with open(_os.devnull, 'w') as stderr: - old_target = _sys.stderr - _sys.stderr = stderr - from .jiminy_pywrap import * - _sys.stderr = old_target +from contextlib import redirect_stderr as _redirect_stderr +with open(_os.devnull, 'w') as stderr, _redirect_stderr(stderr): + from .jiminy_pywrap import * + from .jiminy_pywrap import __version__, __raw_version__