diff --git a/core/src/Utilities.cc b/core/src/Utilities.cc index 8626a4ee1..e6844bfcb 100644 --- a/core/src/Utilities.cc +++ b/core/src/Utilities.cc @@ -774,28 +774,30 @@ namespace jiminy } auto const & joint = model.joints[idIn]; + std::string const & jointTypeStr = joint.shortname(); - if (joint.shortname() == "JointModelFreeFlyer") + if (jointTypeStr == "JointModelFreeFlyer") { jointTypeOut = joint_t::FREE; } - else if (joint.shortname() == "JointModelSpherical") + else if (jointTypeStr == "JointModelSpherical") { jointTypeOut = joint_t::SPHERICAL; } - else if (joint.shortname() == "JointModelPlanar") + else if (jointTypeStr == "JointModelPlanar") { jointTypeOut = joint_t::PLANAR; } - else if (joint.shortname() == "JointModelPX" || - joint.shortname() == "JointModelPY" || - joint.shortname() == "JointModelPZ") + else if (jointTypeStr == "JointModelPX" || + jointTypeStr == "JointModelPY" || + jointTypeStr == "JointModelPZ") { jointTypeOut = joint_t::LINEAR; } - else if (joint.shortname() == "JointModelRX" || - joint.shortname() == "JointModelRY" || - joint.shortname() == "JointModelRZ") + else if (jointTypeStr == "JointModelRX" || + jointTypeStr == "JointModelRY" || + jointTypeStr == "JointModelRZ" || + jointTypeStr == "JointModelRevoluteUnaligned") { jointTypeOut = joint_t::ROTARY; } @@ -803,7 +805,8 @@ namespace jiminy { // Unknown joint, throw an error to avoid any wrong manipulation. jointTypeOut = joint_t::NONE; - std::cout << "Error - Utilities::getJointTypeFromIdx - Unknown joint type." << std::endl; + std::cout << "Error - Utilities::getJointTypeFromIdx - Unknown joint type '" + << jointTypeStr << "'." << std::endl; return hresult_t::ERROR_GENERIC; } @@ -892,7 +895,8 @@ namespace jiminy { if (!model.existFrame(frameName)) { - std::cout << "Error - Utilities::getFrameIdx - Frame not found in urdf." << std::endl; + std::cout << "Error - Utilities::getFrameIdx - Frame '" + << frameName << "' not found in robot model." << std::endl; return hresult_t::ERROR_BAD_INPUT; } @@ -927,7 +931,8 @@ namespace jiminy { if (!model.existBodyName(bodyName)) { - std::cout << "Error - Utilities::getFrameIdx - Frame not found in urdf." << std::endl; + std::cout << "Error - Utilities::getBodyIdx - Body '" + << bodyName << "' not found in robot model." << std::endl; return hresult_t::ERROR_BAD_INPUT; } @@ -964,7 +969,8 @@ namespace jiminy if (!model.existJointName(jointName)) { - std::cout << "Error - Utilities::getJointPositionIdx - Joint not found in urdf." << std::endl; + std::cout << "Error - Utilities::getJointPositionIdx - Joint '" + << jointName << "' not found in robot model." << std::endl; return hresult_t::ERROR_BAD_INPUT; } @@ -985,7 +991,8 @@ namespace jiminy if (!model.existJointName(jointName)) { - std::cout << "Error - Utilities::getJointPositionIdx - Joint not found in urdf." << std::endl; + std::cout << "Error - Utilities::getJointPositionIdx - Joint '" + << jointName << "' not found in robot model." << std::endl; return hresult_t::ERROR_BAD_INPUT; } @@ -1045,7 +1052,8 @@ namespace jiminy if (!model.existJointName(jointName)) { - std::cout << "Error - Utilities::getJointPositionIdx - Joint not found in urdf." << std::endl; + std::cout << "Error - Utilities::getJointModelIdx - Joint '" + << jointName << "' not found in robot model." << std::endl; return hresult_t::ERROR_BAD_INPUT; } @@ -1085,7 +1093,8 @@ namespace jiminy if (!model.existJointName(jointName)) { - std::cout << "Error - getJointVelocityIdx - Frame not found in urdf." << std::endl; + std::cout << "Error - getJointVelocityIdx - Joint '" + << jointName << "' not found in robot model." << std::endl; return hresult_t::ERROR_BAD_INPUT; } @@ -1106,7 +1115,8 @@ namespace jiminy if (!model.existJointName(jointName)) { - std::cout << "Error - getJointVelocityIdx - Frame not found in urdf." << std::endl; + std::cout << "Error - getJointVelocityIdx - Joint '" + << jointName << "' not found in robot model." << std::endl; return hresult_t::ERROR_BAD_INPUT; } diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc index 3e527eac1..0d934e255 100644 --- a/core/src/engine/EngineMultiRobot.cc +++ b/core/src/engine/EngineMultiRobot.cc @@ -1554,8 +1554,8 @@ namespace jiminy || (EPS < controllerUpdatePeriod && controllerUpdatePeriod < SIMULATION_MIN_TIMESTEP) || controllerUpdatePeriod > SIMULATION_MAX_TIMESTEP) { - std::cout << "Error - EngineMultiRobot::setOptions - Cannot simulate a discrete system with update period smaller than" - << SIMULATION_MIN_TIMESTEP << "s or larger than" << SIMULATION_MAX_TIMESTEP << "s. " + std::cout << "Error - EngineMultiRobot::setOptions - Cannot simulate a discrete system with update period smaller than " + << SIMULATION_MIN_TIMESTEP << "s or larger than " << SIMULATION_MAX_TIMESTEP << "s. " << "Increase period or switch to continuous mode by setting period to zero." << std::endl; return hresult_t::ERROR_BAD_INPUT; } diff --git a/core/src/robot/Model.cc b/core/src/robot/Model.cc index 7a8a245d2..bf4d4c2f8 100644 --- a/core/src/robot/Model.cc +++ b/core/src/robot/Model.cc @@ -226,13 +226,6 @@ namespace jiminy return hresult_t::ERROR_INIT_FAILED; } - // Make sure that the body list is not empty - if (bodyNames.empty()) - { - std::cout << "Error - Model::addCollisionBodies - The list of bodies must not be empty." << std::endl; - return hresult_t::ERROR_BAD_INPUT; - } - // Make sure that no body are duplicates if (checkDuplicates(bodyNames)) { @@ -257,24 +250,6 @@ namespace jiminy } } - // Make sure that one and only one geometry is associated with each body - for (std::string const & name : bodyNames) - { - int32_t nChildGeom = 0; - for (pinocchio::GeometryObject const & geom : pncGeometryModel_.geometryObjects) - { - if (pncModel_.frames[geom.parentFrame].name == name) - { - nChildGeom++; - } - } - if (nChildGeom != 1) - { - std::cout << "Error - Model::addCollisionBodies - Collision is only supported for bodies associated with one and only one geometry." << std::endl; - return hresult_t::ERROR_BAD_INPUT; - } - } - // Add the list of bodies to the set of collision bodies collisionBodiesNames_.insert(collisionBodiesNames_.end(), bodyNames.begin(), bodyNames.end()); @@ -282,26 +257,20 @@ namespace jiminy pinocchio::GeomIndex const & groundId = pncGeometryModel_.getGeometryId("ground"); for (std::string const & name : bodyNames) { - // Find the body id by looking at the first geometry having it for parent - pinocchio::GeomIndex bodyId; + // Find the geometries having the body for parent, and add a collision pair for each of them for (uint32_t i=0; i= 4 || PINOCCHIO_PATCH_VERSION >= 4 for (uint32_t i=0; i None: self.robot.set_options(robot_options) self.engine_py.set_engine_options(engine_options) - def _refresh_learning_spaces(self) -> None: + def _refresh_observation_space(self) -> None: """ - @brief Configure the observation and action space of the environment. + @brief Configure the observation of the environment. @details By default, the observation is a dictionary gathering the current simulation time, the real robot state, and the sensors @@ -199,7 +197,6 @@ def _refresh_learning_spaces(self) -> None: ## Extract some proxies joints_position_idx = self.robot.rigid_joints_position_idx joints_velocity_idx = self.robot.rigid_joints_velocity_idx - motors_velocity_idx = self.robot.motors_velocity_idx position_limit_upper = self.robot.position_limit_upper position_limit_lower = self.robot.position_limit_lower velocity_limit = self.robot.velocity_limit @@ -234,13 +231,6 @@ def _refresh_learning_spaces(self) -> None: effort_limit[motor.joint_velocity_idx] = \ MOTOR_EFFORT_UNIVERSAL_MAX - ## Action space - action_low = -effort_limit[motors_velocity_idx] - action_high = +effort_limit[motors_velocity_idx] - - self.action_space = gym.spaces.Box( - low=action_low, high=action_high, dtype=np.float64) - ## Sensor space sensor_space_raw = { key: {'min': np.full(value.shape, -np.inf), @@ -271,11 +261,11 @@ def _refresh_learning_spaces(self) -> None: for sensor_name in sensor_list: sensor = self.robot.get_sensor(effort.type, sensor_name) sensor_idx = sensor.idx - motor_idx = sensor.motor_idx + motor_idx = self.robot.motors_velocity_idx[sensor.motor_idx] sensor_space_raw[effort.type]['min'][0, sensor_idx] = \ - action_low[motor_idx] + -effort_limit[motor_idx] sensor_space_raw[effort.type]['max'][0, sensor_idx] = \ - action_high[motor_idx] + +effort_limit[motor_idx] # Replace inf bounds of the contact sensor space if contact.type in sensors_data.keys(): @@ -339,6 +329,30 @@ def _refresh_learning_spaces(self) -> None: self._observation = {'t': None, 'state': None, 'sensors': None} + def _refresh_action_space(self) -> None: + """ + @brief Configure the action space of the environment. + + @details By default, the action is a vector gathering the torques of + the actuator of the robot. + + @remark This method is called internally by 'reset' method. + """ + # Replace inf bounds of the effort limit + effort_limit = self.robot.effort_limit + for motor_name in self.robot.motors_names: + motor = self.robot.get_motor(motor_name) + motor_options = motor.get_options() + if not motor_options["enableEffortLimit"]: + effort_limit[motor.joint_velocity_idx] = \ + MOTOR_EFFORT_UNIVERSAL_MAX + + # Set the action space + self.action_space = gym.spaces.Box( + low=-effort_limit[self.robot.motors_velocity_idx], + high=effort_limit[self.robot.motors_velocity_idx], + dtype=np.float64) + def _sample_state(self) -> Tuple[np.ndarray, np.ndarray]: """ @brief Returns a random valid configuration and velocity for the robot. @@ -368,7 +382,7 @@ def _update_obs(self, obs: SpaceDictRecursive) -> None: @details By default, no filtering is applied on the raw data extracted from the engine. - @remark This method, alongside '_refresh_learning_spaces', must be + @remark This method, alongside '_refresh_observation_space', must be overwritten in order to use a custom observation space. """ obs['t'] = self.engine_py.t @@ -469,9 +483,12 @@ def set_state(self, qpos: np.ndarray, qvel: np.ndarray) -> None: self._steps_beyond_done = None self._log_data = None - # Clear the log file + # Create a new log file if self.debug is not None: - self._log_file.truncate(0) + if self._log_file is not None: + self._log_file.close() + self._log_file = tempfile.NamedTemporaryFile( + prefix="log_", suffix=".data", delete=(not self.debug)) def reset(self) -> SpaceDictRecursive: """ @@ -488,8 +505,9 @@ def reset(self) -> SpaceDictRecursive: self.set_state(*self._sample_state()) # Refresh the observation and action spaces - self._refresh_learning_spaces() + self._refresh_observation_space() self._update_obs(self._observation) + self._refresh_action_space() return self._get_obs() @@ -635,10 +653,11 @@ def __init__(self, ## Sample a new goal self.goal = self._sample_goal() - def _refresh_learning_spaces(self) -> None: - super()._refresh_learning_spaces() + def _refresh_observation_space(self) -> None: + # Initialize the original observation space first + super()._refresh_observation_space() - ## Append default desired and achieved goal spaces to observation space + # Append default desired and achieved goal spaces to observation space self.observation_space = gym.spaces.Dict( desired_goal=gym.spaces.Box( -np.inf, np.inf, shape=self.goal.shape, dtype=np.float64), @@ -646,7 +665,7 @@ def _refresh_learning_spaces(self) -> None: -np.inf, np.inf, shape=self.goal.shape, dtype=np.float64), observation=self.observation_space) - ## Current observation of the robot + # Current observation of the robot self.observation = {'observation': self.observation, 'achieved_goal': None, 'desired_goal': None} diff --git a/gym_jiminy/gym_jiminy/common/env_locomotion.py b/gym_jiminy/gym_jiminy/common/env_locomotion.py index f5b3585ae..cbffe3264 100644 --- a/gym_jiminy/gym_jiminy/common/env_locomotion.py +++ b/gym_jiminy/gym_jiminy/common/env_locomotion.py @@ -2,7 +2,7 @@ import numpy as np import numba as nb -from typing import Optional +from typing import Optional, Union from jiminy_py.core import (EncoderSensor as encoder, EffortSensor as effort, @@ -15,8 +15,9 @@ import pinocchio as pin from pinocchio import neutral -from .env_base import BaseJiminyEnv +from .env_bases import BaseJiminyEnv from .distributions import PeriodicGaussianProcess +from .wrappers import flatten_observation MIN_GROUND_STIFFNESS_LOG = 5.5 @@ -47,12 +48,12 @@ DEFAULT_SIMULATION_DURATION = 20.0 # (s) Default simulation duration DEFAULT_ENGINE_DT = 1.0e-3 # (s) Stepper update period -DEFAULT_HLC_TO_LLC_RATIO = 1 # (NA) +DEFAULT_HLC_TO_LLC_RATIO = 1 # (NA) PID_KP = 20000.0 PID_KD = 0.01 -class WalkerTorqueControlJiminyEnv(BaseJiminyEnv): +class WalkerJiminyEnv(BaseJiminyEnv): """ @brief Implementation of a Gym environment for learning locomotion task for legged robots. It uses Jiminy Engine to perform physics evaluation @@ -71,7 +72,8 @@ def __init__(self, dt: float = DEFAULT_ENGINE_DT, reward_mixture: Optional[dict] = None, std_ratio: Optional[dict] = None, - debug: bool = False): + debug: bool = False, + **kwargs): """ @brief Constructor. @@ -126,7 +128,7 @@ def __init__(self, self._power_consumption_max = None # Configure and initialize the learning environment - super().__init__(None, dt, debug) + super().__init__(None, dt, debug, **kwargs) def _setup_environment(self): """ @@ -137,7 +139,8 @@ def _setup_environment(self): # Instantiate a new engine self.engine_py = BaseJiminyEngine( self.urdf_path, self.toml_path, self.mesh_path, - has_freeflyer=True, use_theoretical_model=False) + has_freeflyer=True, use_theoretical_model=False, + debug=self.debug) # Discard log data since no longer relevant self._log_data = None @@ -291,15 +294,15 @@ def _is_done(self): """ # Simulation termination conditions: # - Fall detection (if the robot has a freeflyer): - # The freeflyer goes lower than 90% of its height in standing pose. + # The freeflyer goes lower than 75% of its height in standing pose. # - Maximum simulation duration exceeded return (self.robot.has_freeflyer and - self.engine_py.state[2] < self._height_neutral * 0.9) or \ + self.engine_py.state[2] < self._height_neutral * 0.75) or \ (self.engine_py.t >= self.simu_duration_max) def _compute_reward(self): # @copydoc BaseJiminyRobot::_compute_reward - reward = {} + reward_dict = {} # Define some proxies reward_mixture_keys = self.reward_mixture.keys() @@ -309,24 +312,28 @@ def _compute_reward(self): power_consumption = sum(np.maximum(self.action_prev * v_mot, 0.0)) power_consumption_rel = \ power_consumption / self._power_consumption_max - reward['energy'] = - power_consumption_rel + reward_dict['energy'] = - power_consumption_rel if 'done' in reward_mixture_keys: - reward['done'] = 1 + reward_dict['done'] = 1 # Rescale the reward so that the maximum cumulative reward is # independent from both the engine timestep and the maximum # simulation duration. - reward = {k: v * (self.dt / self.simu_duration_max) - for k, v in reward.items()} + reward_dict = {k: v * (self.dt / self.simu_duration_max) + for k, v in reward_dict.items()} + + # Compute the total reward + reward_total = sum([self.reward_mixture[name] * value + for name, value in reward_dict.items()]) - return reward + return reward_total, reward_dict def _compute_reward_terminal(self): """ @brief Compute the reward at the end of the episode. """ - reward = {} + reward_dict = {} reward_mixture_keys = self.reward_mixture.keys() @@ -339,9 +346,13 @@ def _compute_reward_terminal(self): 'HighLevelController.currentFreeflyerPositionTransY'])) else: frontal_displacement = 0.0 - reward['direction'] = - frontal_displacement + reward_dict['direction'] = - frontal_displacement + + # Compute the total reward + reward_total = sum([self.reward_mixture[name] * value + for name, value in reward_dict.items()]) - return reward + return reward_total, reward_dict def step(self, action): """ @@ -349,12 +360,12 @@ def step(self, action): """ # Perform a single simulation step try: - obs, reward_info, done, info = super().step(action) + obs, reward, done, info = super().step(action) except RuntimeError: obs = self.observation + reward = 0.0 done = True info = {'is_success': False} - reward_info = {} if done: # Extract the log data from the simulation @@ -364,21 +375,10 @@ def step(self, action): if self.debug and self._steps_beyond_done == 0: self.engine.write_log(self.log_path) - # Add the final reward if the simulation is over - reward_final = self._compute_reward_terminal() - reward_info.update(reward_final) - - # Compute the total reward - reward = sum([self.reward_mixture[name] * value - for name, value in reward_info.items()]) - - # Extract additional information - info.update({'reward': reward_info}) - return obs, reward, done, info -class WalkerPDControlJiminyEnv(WalkerTorqueControlJiminyEnv): +class WalkerPDControlJiminyEnv(WalkerJiminyEnv): def __init__(self, urdf_path: str, toml_path: Optional[str] = None, @@ -386,11 +386,15 @@ def __init__(self, simu_duration_max: float = DEFAULT_SIMULATION_DURATION, dt: float = DEFAULT_ENGINE_DT, hlc_to_llc_ratio: int = DEFAULT_HLC_TO_LLC_RATIO, + pid_kp: Union[float, np.ndarray] = PID_KP, + pid_kd: Union[float, np.ndarray] = PID_KD, reward_mixture: Optional[dict] = None, std_ratio: Optional[dict] = None, debug: bool = False): # 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._q_target = None @@ -400,6 +404,13 @@ def __init__(self, super().__init__(urdf_path, toml_path, mesh_path, simu_duration_max, dt, reward_mixture, std_ratio, debug) + def _refresh_action_space(self): + """ + @brief TODO + """ + self.action_space = flatten_observation( + self.observation_space['sensors']['EncoderSensor']) + def _compute_command(self): """ @brief TODO @@ -408,8 +419,9 @@ def _compute_command(self): q_enc, v_enc = self.engine_py.sensors_data[encoder.type] # Compute PD command - u = - PID_KP * ((q_enc - self._q_target) + \ - PID_KD * (v_enc - self._v_target)) + u = - self.pid_kp * ((q_enc - self._q_target) + \ + self.pid_kd * (v_enc - self._v_target)) + return u def step(self, action): @@ -421,12 +433,12 @@ def step(self, action): # Run the whole simulation in one go reward = 0.0 - reward_info = {k: 0.0 for k in self.reward_mixture.keys()} + reward_info = {k: [] for k in self.reward_mixture.keys()} 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(): - reward_info[k] += v + reward_info[k].append(v) reward += reward_step if done: break diff --git a/gym_jiminy/gym_jiminy/envs/__init__.py b/gym_jiminy/gym_jiminy/envs/__init__.py index 0148da070..02b456185 100644 --- a/gym_jiminy/gym_jiminy/envs/__init__.py +++ b/gym_jiminy/gym_jiminy/envs/__init__.py @@ -1,2 +1,3 @@ -from .cartpole import JiminyCartPoleEnv -from .acrobot import JiminyAcrobotEnv, JiminyAcrobotGoalEnv \ No newline at end of file +from .cartpole import CartPoleJiminyEnv +from .acrobot import AcrobotJiminyEnv, AcrobotJiminyGoalEnv + diff --git a/gym_jiminy/gym_jiminy/envs/acrobot.py b/gym_jiminy/gym_jiminy/envs/acrobot.py index 4e81805eb..81ddae223 100644 --- a/gym_jiminy/gym_jiminy/envs/acrobot.py +++ b/gym_jiminy/gym_jiminy/envs/acrobot.py @@ -7,9 +7,9 @@ from gym import spaces from jiminy_py import core as jiminy -from jiminy_py.engine_asynchronous import EngineAsynchronous +from jiminy_py.engine import EngineAsynchronous -from ..common.robots import BaseJiminyEnv, BaseJiminyGoalEnv +from ..common.env_bases import BaseJiminyGoalEnv DT = 2.0e-3 ## Stepper update period @@ -59,8 +59,6 @@ def __init__(self, continuous=False): If not continuous, the action space has only 3 states, i.e. low, zero, and high. Optional: True by default. - - @return Instance of the environment. """ # @copydoc BaseJiminyEnv::__init__ @@ -77,9 +75,9 @@ def __init__(self, continuous=False): # ############################### Initialize Jiminy #################################### os.environ["JIMINY_DATA_PATH"] = \ - resource_filename('gym_jiminy.envs', 'data') + resource_filename('gym_jiminy.envs', 'data/toys_models') urdf_path = os.path.join(os.environ["JIMINY_DATA_PATH"], - "double_pendulum/double_pendulum.urdf") + "toys_models/double_pendulum/double_pendulum.urdf") robot = jiminy.Robot() robot.initialize(urdf_path) diff --git a/gym_jiminy/gym_jiminy/envs/cartpole.py b/gym_jiminy/gym_jiminy/envs/cartpole.py index 42c2025fd..83deaf4a3 100644 --- a/gym_jiminy/gym_jiminy/envs/cartpole.py +++ b/gym_jiminy/gym_jiminy/envs/cartpole.py @@ -4,12 +4,12 @@ import numpy as np from pkg_resources import resource_filename -from gym import spaces, logger +from gym import spaces from jiminy_py import core as jiminy -from jiminy_py.engine_asynchronous import EngineAsynchronous +from jiminy_py.engine import EngineAsynchronous -from ..common.robots import BaseJiminyEnv +from ..common.env_bases import BaseJiminyEnv DT = 2.0e-3 ## Stepper update period @@ -65,12 +65,6 @@ class CartPoleJiminyEnv(BaseJiminyEnv): equal to 195.0 over 100 consecutive trials. """ def __init__(self, continuous=False): - """ - @brief Constructor - - @return Instance of the environment. - """ - # @copydoc BaseJiminyEnv::__init__ # ## @var state_random_high # @copydoc BaseJiminyEnv::state_random_high @@ -85,9 +79,9 @@ def __init__(self, continuous=False): # ############################### Initialize Jiminy #################################### os.environ["JIMINY_DATA_PATH"] = \ - resource_filename('gym_jiminy.envs', 'data') + resource_filename('gym_jiminy.envs', 'data/toys_models') urdf_path = os.path.join(os.environ["JIMINY_DATA_PATH"], - "cartpole/cartpole.urdf") + "toys_models/cartpole/cartpole.urdf") robot = jiminy.Robot() robot.initialize(urdf_path) diff --git a/python/jiminy_py/setup.py b/python/jiminy_py/setup.py index 79661248b..cc7f78232 100644 --- a/python/jiminy_py/setup.py +++ b/python/jiminy_py/setup.py @@ -28,7 +28,9 @@ def finalize_options(self): download_url = 'https://github.com/Wandercraft/jiminy/archive/@PROJECT_VERSION@.tar.gz', packages = find_packages('src'), package_dir = {'': 'src'}, - package_data = {'jiminy_py': ['**/*.dll', '**/*.so', '**/*.pyd', '**/*.html', '**/*.js']}, + package_data = {'jiminy_py': [ + '**/*.dll', '**/*.so', '**/*.pyd', '**/*.html', '**/*.js' + ]}, entry_points = {'console_scripts': [ 'jiminy_plot=jiminy_py.log:plot_log', 'jiminy_meshcat_server=jiminy_py.meshcat.server:start_meshcat_server_standalone' diff --git a/python/jiminy_py/src/jiminy_py/engine.py b/python/jiminy_py/src/jiminy_py/engine.py index 0f4a7aca0..a09c65d22 100644 --- a/python/jiminy_py/src/jiminy_py/engine.py +++ b/python/jiminy_py/src/jiminy_py/engine.py @@ -1,10 +1,12 @@ ## @file src/jiminy_py/engine.py +import os +import pathlib import tempfile import numpy as np from typing import Optional, List, Any from . import core as jiminy -from .robot import BaseJiminyRobot +from .robot import generate_hardware_description_file, BaseJiminyRobot from .controller import BaseJiminyController from .viewer import Viewer from .dynamics import update_quantities @@ -385,10 +387,20 @@ def __init__(self, mesh_path: Optional[str] = None, has_freeflyer: bool = True, use_theoretical_model: bool = False, - viewer_backend: Optional[str] = None): + viewer_backend: Optional[str] = None, + debug: bool = False): """ @brief TODO """ + # Generate a temporary Hardware Description File if necessary + if toml_path is None: + toml_path = pathlib.Path(urdf_path).with_suffix('.toml') + if not os.path.exists(toml_path): + self._toml_file = tempfile.NamedTemporaryFile( + prefix="anymal_hdf_", suffix=".toml", delete=(not debug)) + toml_path = self._toml_file.name + generate_hardware_description_file(urdf_path, toml_path) + # Instantiate and initialize the robot robot = BaseJiminyRobot() robot.initialize(urdf_path, toml_path, mesh_path, has_freeflyer) @@ -400,10 +412,17 @@ def __init__(self, use_theoretical_model, viewer_backend) - # Set engine controller and sensor update period if available + # Set some engine options, based on extra toml information engine_options = self.get_options() - engine_options["stepper"]["controllerUpdatePeriod"] = \ - robot.global_info.get('sensorsUpdatePeriod', 0.0) - engine_options["stepper"]["sensorsUpdatePeriod"] = \ - robot.global_info.get('sensorsUpdatePeriod', 0.0) + engine_options['stepper']['controllerUpdatePeriod'] = \ + robot.extra_info.pop('sensorsUpdatePeriod', 0.0) + engine_options['stepper']['sensorsUpdatePeriod'] = \ + robot.extra_info.pop('sensorsUpdatePeriod', 0.0) + engine_options['contacts']['stiffness'] = \ + robot.extra_info.pop('groundStiffness', 4.0e6) + engine_options['contacts']['damping'] = \ + robot.extra_info.pop('groundDamping', 2.0e3) self.set_options(engine_options) + + def __del__(self): + self._toml_file.close() diff --git a/python/jiminy_py/src/jiminy_py/log.py b/python/jiminy_py/src/jiminy_py/log.py index 0cf4c159a..b195157b2 100644 --- a/python/jiminy_py/src/jiminy_py/log.py +++ b/python/jiminy_py/src/jiminy_py/log.py @@ -114,7 +114,7 @@ def plot_log(): linestyles = ["--","-.",":"] # Parse plotting arguments. - plotted_elements = [ + plotted_elements = [] for cmd in plotting_commands: # Check that the command is valid, i.e. that all elements exits. # If it is the case, add it to the list. diff --git a/python/jiminy_py/src/jiminy_py/robot.py b/python/jiminy_py/src/jiminy_py/robot.py index d04114cd3..49421d3de 100644 --- a/python/jiminy_py/src/jiminy_py/robot.py +++ b/python/jiminy_py/src/jiminy_py/robot.py @@ -8,6 +8,7 @@ import tempfile import numpy as np import xml.etree.ElementTree as ET +from fractions import gcd from collections import OrderedDict from typing import Optional @@ -31,26 +32,26 @@ def generate_hardware_description_file( urdf_path: str, toml_path: Optional[str] = None, - default_update_rate: Optional[float] = None): + default_update_rate: Optional[float] = DEFAULT_UPDATE_RATE): """ @brief Generate a default hardware description file, based on the information grabbed from the URDF when available, using best guess otherwise. - @details If no Gazebo plugin is available, a single IMU is added on the - root body, and force sensors are added on every leaf of the robot - kinematic tree. Otherwise, the definition of the plugins in use - to infer them. + @details If no Gazebo IMU sensor is found, a single IMU is added on the + root body of the kinematic tree. If no Gazebo plugin is available, + force sensors are added on every leaf bodies of the robot. + Otherwise, the definition of the plugins in use to infer them. - Joint fields are parsed to extract the every joints, actuated - or not. Fixed joints are not considered as actual joints. + 'joint' fields are parsed to extract the every joints, actuated + or not. 'fixed' joints are not considered as actual joints. Transmission fields are parsed to determine which one of those joints are actuated. If no transmission is found, it is assumed that every joint is actuated, with a transmission ratio of 1:1. - It is assumed that every joints have an encoder attached, as it is - the case in Gazebo. Every actuated joint have an effort sensor - attached by default. + It is assumed that every joints have an encoder attached. Every + actuated joint have an effort sensor attached by default. In + addition, every collision bodies have a force sensor attached. When the default update rate is unspecified, then the default sensor update rate is 1KHz if no Gazebo plugin has been found, @@ -76,19 +77,42 @@ def generate_hardware_description_file( root = tree.getroot() # Initialize the hardware information - hardware_info = OrderedDict() - hardware_info['Global'] = OrderedDict() - hardware_info['Motor'] = OrderedDict() - hardware_info['Sensor'] = OrderedDict() + hardware_info = OrderedDict( + Global=OrderedDict( + sensorsUpdatePeriod=1.0/default_update_rate, + controllerUpdatePeriod=1.0/default_update_rate, + collisionBodiesNames=[], + contactFramesNames=[] + ), + Motor=OrderedDict(), + Sensor=OrderedDict() + ) + + # Extract the list of parent and child links, excluding the one related + # to fixed joints, because they are likely not "real" joint. + parent_links = set() + child_links = set() + for joint_descr in root.findall('./joint'): + if joint_descr.get('type').casefold() != 'fixed': + parent_links.add(joint_descr.find('./parent').get('link')) + child_links.add(joint_descr.find('./child').get('link')) + + # Compute the root link and the leaf ones + root_links = list(parent_links.difference(child_links)) + leaf_links = list(child_links.difference(parent_links)) # Parse the gazebo plugins, if any. # Note that it is only useful to extract "advanced" hardware, not basic # motors, encoders and effort sensors. + gazebo_ground_stiffness = None + gazebo_ground_damping = None gazebo_update_rate = None + collision_bodies_names = set() gazebo_plugins_found = root.find('gazebo') is not None for gazebo_plugin_descr in root.iterfind('gazebo'): body_name = gazebo_plugin_descr.get('reference') + # Extract sensors for gazebo_sensor_descr in gazebo_plugin_descr.iterfind('sensor'): sensor_info = OrderedDict(body_name=body_name) @@ -100,10 +124,11 @@ def generate_hardware_description_file( if 'imu' in sensor_type: sensor_type = imu.type elif 'contact' in sensor_type: + collision_bodies_names.add(body_name) sensor_type = force.type else: - logger.warning( - f'Unsupported Gazebo sensor plugin {gazebo_sensor_descr}') + logger.warning("Unsupported Gazebo sensor plugin of type " + f"'{sensor_type}'") continue # Extract the sensor update period @@ -112,9 +137,10 @@ def generate_hardware_description_file( gazebo_update_rate = update_rate else: if gazebo_update_rate != update_rate: - logger.warning("Jiminy does not support sensors with" - "different update rate. Using the highest one.") - gazebo_update_rate = update_rate + logger.warning("Jiminy does not support sensors with " + "different update rate. Using greatest common " + "divisor instead.") + gazebo_update_rate = gcd(gazebo_update_rate, update_rate) # Extract the pose of the frame associate with the sensor. # Note that it is optional but usually defined since sensors @@ -130,22 +156,41 @@ def generate_hardware_description_file( hardware_info['Sensor'].setdefault(sensor_type, {}).update( {sensor_name: sensor_info}) - # Fallback if no Gazebo plugin has been found - if not gazebo_plugins_found: - # Extract the list of parent and child links, excluding the one related - # to fixed joints, because they are likely not "real" joint. - parent_links = set() - child_links = set() - for joint_descr in root.findall('./joint'): - if joint_descr.get('type').casefold() != 'fixed': - parent_links.add(joint_descr.find('./parent').get('link')) - child_links.add(joint_descr.find('./child').get('link')) - - # Compute the root link and the leaf ones - root_links = list(parent_links.difference(child_links)) - leaf_links = list(child_links.difference(parent_links)) - - # Add IMU sensor to the root link + # Extract the collision bodies and ground model, then add force sensors + if gazebo_plugin_descr.find('kp') is not None: + # Add a force sensor, if not already in the collision set + if not body_name in collision_bodies_names: + hardware_info['Sensor'].setdefault(force.type, {}).update({ + f"{body_name}Contact": OrderedDict( + body_name=body_name, + frame_pose=6*[0.0]) + }) + + # Add the related body to the collision set + collision_bodies_names.add(body_name) + + # Update the ground model + ground_stiffness = float(gazebo_plugin_descr.find('kp').text) + ground_damping = float(gazebo_plugin_descr.find('kd').text) + if gazebo_ground_stiffness is None: + gazebo_ground_stiffness = ground_stiffness + if gazebo_ground_damping is None: + gazebo_ground_damping = ground_damping + if (gazebo_ground_stiffness != ground_stiffness or + gazebo_ground_damping != ground_damping): + raise RuntimeError("Jiminy does not support contacts with" + "different ground models.") + + # Specify collision bodies and ground model in global config options + hardware_info['Global']['collisionBodiesNames'] = \ + list(collision_bodies_names) + if gazebo_ground_stiffness is not None: + hardware_info['Global']['groundStiffness'] = gazebo_ground_stiffness + if gazebo_ground_damping is not None: + hardware_info['Global']['groundDamping'] = gazebo_ground_damping + + # Add IMU sensor to the root link if no Gazebo IMU sensor has been found + if not imu.type in hardware_info['Sensor'].keys(): for root_link in root_links: hardware_info['Sensor'].setdefault(imu.type, {}).update({ root_link: OrderedDict( @@ -154,7 +199,8 @@ def generate_hardware_description_file( ) }) - # Add force sensors to the leaf links + # Add force sensors if no Gazebo plugin is available at all + if not gazebo_plugins_found: for leaf_link in leaf_links: hardware_info['Sensor'].setdefault(force.type, {}).update({ leaf_link: OrderedDict( @@ -163,7 +209,7 @@ def generate_hardware_description_file( ) }) - # Extract the effort sensors. + # Extract the motors and effort sensors. # It is done by reading 'transmission' field, that is part of # URDF standard, so it should be available on any URDF file. transmission_found = root.find('transmission') is not None @@ -179,6 +225,15 @@ def generate_hardware_description_file( joint_name = transmission_descr.find('./joint').get('name') motor_info['joint_name'] = joint_name + # Make sure that the joint is revolute + joint_type = root.find( + f"./joint[@name='{joint_name}']").get("type").casefold() + if joint_type != "revolute": + logger.warning( + "Jiminy only support revolute actuators and effort sensors. " + f"Attached joint cannot of type '{joint_type}'.") + continue + # Extract the transmission ratio (motor / joint) ratio = transmission_descr.find('./actuator/mechanicalReduction') if ratio is None: @@ -220,21 +275,19 @@ def generate_hardware_description_file( # Add motors to robot hardware by default if no transmission found if not transmission_found: - hardware_info['Sensor'].setdefault('SimpleMotor', {}).update( + hardware_info['Motor'].setdefault('SimpleMotor', {}).update( {name: OrderedDict( joint_name=name, mechanicalReduction=1.0, rotorInertia=0.0) }) - # Handling of default update rate for the controller and the sensors - if gazebo_update_rate is None: - if default_update_rate is not None: - gazebo_update_rate = default_update_rate - else: - gazebo_update_rate = DEFAULT_UPDATE_RATE - hardware_info['Global']['sensorsUpdatePeriod'] = 1 / gazebo_update_rate - hardware_info['Global']['controllerUpdatePeriod'] = 1 / gazebo_update_rate + # Specify custom update rate for the controller and the sensors, if any + if gazebo_update_rate is not None: + hardware_info['Global']['sensorsUpdatePeriod'] = \ + 1.0 / gazebo_update_rate + hardware_info['Global']['controllerUpdatePeriod'] = \ + 1.0 / gazebo_update_rate # Write the sensor description file if toml_path is None: @@ -317,7 +370,7 @@ def __init__(self): @brief TODO """ super().__init__() - self.global_info = {} + self.extra_info = {} self.robot_options = None self.urdf_path_orig = None @@ -350,12 +403,13 @@ def initialize(self, urdf_path = fix_urdf_mesh_path(urdf_path, mesh_path) # Initialize the robot without motors nor sensors - mesh_root_dirs = [] if mesh_path is not None: - mesh_root_dirs += [mesh_path] + mesh_root_dirs = [mesh_path] + else: + mesh_root_dirs = [os.path.dirname(urdf_path)] mesh_env_path = os.environ.get('JIMINY_DATA_PATH', None) if mesh_env_path is not None: - mesh_path += [mesh_env_path] + mesh_root_dirs += [mesh_env_path] return_code = super().initialize(urdf_path, has_freeflyer, mesh_root_dirs) if return_code != jiminy.hresult_t.SUCCESS: @@ -371,7 +425,7 @@ def initialize(self, "automatically using 'generate_hardware_description_file'.") return hardware_info = toml.load(toml_path) - self.global_info = hardware_info.pop('Global') + self.extra_info = hardware_info.pop('Global') motors_info = hardware_info.pop('Motor') sensors_info = hardware_info.pop('Sensor') @@ -397,7 +451,6 @@ def initialize(self, motor.set_options(options) # Add the sensors to the robot - collision_bodies_names = [] for sensor_type, sensors_descr in sensors_info.items(): for sensor_name, sensor_descr in sensors_descr.items(): # Create the sensor and attach it @@ -425,10 +478,6 @@ def initialize(self, ## Get the body name body_name = sensor_descr.pop('body_name') - ## Add the body to the list for collision bodies if it is a force sensor - if sensor_type in force.type: - collision_bodies_names.append(body_name) - ## Generate a frame name that is intelligible and available i = 0 frame_name = sensor_name + "Frame" @@ -463,7 +512,10 @@ def initialize(self, sensor.set_options(options) # Add the contact points + collision_bodies_names = self.extra_info.pop('collisionBodiesNames', []) self.add_collision_bodies(collision_bodies_names) + contact_frames_names = self.extra_info.pop('contactFramesNames', []) + self.add_contact_points(contact_frames_names) def __del__(self): if self.urdf_path != self.urdf_path_orig: