def load_urdf_default(urdf_path, motor_names, has_freeflyer = False): """ @brief Create a jiminy.Robot from a URDF with several simplying hypothesis. @details The goal of this function is to ease creation of jiminy.Robot from a URDF by doing the following operations: - loading the URDF, deactivating joint position and velocity bounds. - adding motors as supplied, with no rotor inertia and not torque bound. These operations allow an unconstraint simulation of a linear system. @param[in] urdf_path Path to the URDF file @param[in] motor_names Name of the motors @param[in] has_freeflyer Optional, set the use of a freeflyer joint. """ robot = jiminy.Robot() robot.initialize(urdf_path, has_freeflyer = has_freeflyer) for joint_name in motor_names: motor = jiminy.SimpleMotor(joint_name) robot.attach_motor(motor) motor.initialize(joint_name) # Configure robot model_options = robot.get_model_options() motor_options = robot.get_motors_options() model_options["joints"]["enablePositionLimit"] = False model_options["joints"]["enableVelocityLimit"] = False for m in motor_options: motor_options[m]['enableEffortLimit'] = False motor_options[m]['enableRotorInertia'] = False robot.set_model_options(model_options) robot.set_motors_options(motor_options) return robot
def __init__(self, continuous: bool = False, debug: bool = False) -> None: """ :param continuous: Whether or not the action space is continuous. If not continuous, the action space has only 3 states, i.e. low, zero, and high. Optional: True by default. """ # Backup some input arguments self.continuous = continuous # Get URDF path data_dir = resource_filename( "gym_jiminy.envs", "data/toys_models/acrobot") urdf_path = os.path.join( data_dir, "acrobot.urdf") # Instantiate robot robot = jiminy.Robot() robot.initialize( urdf_path, has_freeflyer=False, mesh_package_dirs=[data_dir]) # Add motors and sensors motor_joint_name = "SecondArmJoint" encoder_joint_names = ("FirstArmJoint", "SecondArmJoint") motor = jiminy.SimpleMotor(motor_joint_name) robot.attach_motor(motor) motor.initialize(motor_joint_name) for joint_name in encoder_joint_names: encoder = jiminy.EncoderSensor(joint_name) robot.attach_sensor(encoder) encoder.initialize(joint_name) # Instantiate simulator simulator = Simulator(robot) # Map between discrete actions and actual motor torque if necessary if not self.continuous: self.AVAIL_CTRL = [-motor.command_limit, 0.0, motor.command_limit] # Internal parameters used for computing termination condition self._tipIdx = robot.pinocchio_model.getFrameId("Tip") self._tipPosZMax = abs(robot.pinocchio_data.oMf[ self._tipIdx].translation[[2]]) # Configure the learning environment super().__init__(simulator, step_dt=STEP_DT, debug=debug) # Create some proxies for fast access self.__state_view = (self._observation[:self.robot.nq], self._observation[-self.robot.nv:])
def load_urdf_default(urdf_name: str, motor_names: Sequence[str] = (), has_freeflyer: bool = False) -> jiminy.Robot: """ @brief Create a jiminy.Robot from a URDF with several simplying hypothesis. @details The goal of this function is to ease creation of jiminy.Robot from a URDF by doing the following operations: - loading the URDF, deactivating joint position and velocity bounds. - adding motors as supplied, with no rotor inertia and not torque bound. These operations allow an unconstrained simulation of a linear system. @param urdf_name Name to the URDF file. @param motor_names Name of the motors. @param has_freeflyer Set the use of a freeflyer joint. Optional, no free-flyer by default. """ # Get the urdf path and mesh search directory current_dir = os.path.dirname(os.path.realpath(__file__)) data_root_dir = os.path.join(current_dir, "data") urdf_path = os.path.join(data_root_dir, urdf_name) # Create and initialize the robot robot = jiminy.Robot() robot.initialize(urdf_path, has_freeflyer, [data_root_dir]) # Add motors to the robot for joint_name in motor_names: motor = jiminy.SimpleMotor(joint_name) robot.attach_motor(motor) motor.initialize(joint_name) # Configure the robot model_options = robot.get_model_options() motor_options = robot.get_motors_options() model_options["joints"]["enablePositionLimit"] = False model_options["joints"]["enableVelocityLimit"] = False for m in motor_options: motor_options[m]['enableCommandLimit'] = False motor_options[m]['enableArmature'] = False robot.set_model_options(model_options) robot.set_motors_options(motor_options) return robot
def __init__(self, continuous=False): """ @brief Constructor @return Instance of the environment. """ # @copydoc RobotJiminyEnv::__init__ # ## @var state_random_high # @copydoc RobotJiminyEnv::state_random_high ## @var state_random_low # @copydoc RobotJiminyEnv::state_random_low # ########################## Backup the input arguments ################################ ## Flag to determine if the action space is continuous self.continuous = continuous # ############################### Initialize Jiminy #################################### os.environ["JIMINY_MESH_PATH"] = resource_filename( 'gym_jiminy.envs', 'data') urdf_path = os.path.join(os.environ["JIMINY_MESH_PATH"], "cartpole/cartpole.urdf") robot = jiminy.Robot() robot.initialize(urdf_path) motor_joint_name = "slider_to_cart" encoder_sensors_def = { "slider": "slider_to_cart", "pole": "cart_to_pole" } motor = jiminy.SimpleMotor(motor_joint_name) robot.attach_motor(motor) motor.initialize(motor_joint_name) for sensor_name, joint_name in encoder_sensors_def.items(): encoder = jiminy.EncoderSensor(sensor_name) robot.attach_sensor(encoder) encoder.initialize(joint_name) engine_py = EngineAsynchronous(robot) # ##################### Define some problem-specific variables ######################### if not self.continuous: ## Map between discrete actions and actual motor force self.AVAIL_FORCE = [-MAX_FORCE, MAX_FORCE] ## Maximum absolute angle of the pole before considering the episode failed self.theta_threshold_radians = 25 * pi / 180 ## Maximum absolute position of the cart before considering the episode failed self.x_threshold = 0.75 # Bounds of the hypercube associated with the initial state of the robot self.state_random_high = np.array([0.5, 0.15, 0.1, 0.1]) self.state_random_low = -self.state_random_high # ####################### Configure the learning environment ########################### super().__init__("cartpole", engine_py, DT)
# ################################ User parameters ####################################### script_dir = os.path.dirname(os.path.realpath(__file__)) os.environ["JIMINY_MESH_PATH"] = os.path.join(script_dir, "../../data") urdf_path = os.path.join(os.environ["JIMINY_MESH_PATH"], "double_pendulum/double_pendulum.urdf") # ########################### Initialize the simulation ################################# # Instantiate the robot motor_joint_names = ("SecondPendulumJoint", ) robot = jiminy.Robot() robot.initialize(urdf_path, False) for joint_name in motor_joint_names: motor = jiminy.SimpleMotor(joint_name) robot.attach_motor(motor) motor.initialize(joint_name) # Instantiate the controller def computeCommand(t, q, v, sensors_data, u): u[0] = 0.0 def internalDynamics(t, q, v, sensors_data, u): u[:] = 0.0 controller = jiminy.ControllerFunctor(computeCommand, internalDynamics) controller.initialize(robot)
def __init__(self, continuous=False): """ @brief Constructor @param[in] continuous Whether the action space is continuous or not. 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 RobotJiminyEnv::__init__ ## @var state_random_high # @copydoc RobotJiminyGoalEnv::state_random_high ## @var state_random_low # @copydoc RobotJiminyGoalEnv::state_random_low # ########################## Backup the input arguments ################################ ## Flag to determine if the action space is continuous self.continuous = continuous # ############################### Initialize Jiminy #################################### os.environ["JIMINY_MESH_PATH"] = resource_filename( 'gym_jiminy.envs', 'data') urdf_path = os.path.join(os.environ["JIMINY_MESH_PATH"], "double_pendulum/double_pendulum.urdf") robot = jiminy.Robot() robot.initialize(urdf_path) motor_joint_name = "SecondPendulumJoint" encoder_joint_names = ("PendulumJoint", "SecondPendulumJoint") motor = jiminy.SimpleMotor(motor_joint_name) robot.attach_motor(motor) motor.initialize(motor_joint_name) for joint_name in encoder_joint_names: encoder = jiminy.EncoderSensor(joint_name) robot.attach_sensor(encoder) encoder.initialize(joint_name) engine_py = EngineAsynchronous(robot) # ############################### Configure Jiminy ##################################### robot_options = robot.get_options() # Set the position and velocity bounds of the robot robot_options["model"]["joints"]["velocityLimitFromUrdf"] = False robot_options["model"]["joints"]["velocityLimit"] = MAX_VEL * np.ones( 2) # Set the effort limit of the motor robot_options["motors"][motor_joint_name][ "effortLimitFromUrdf"] = False robot_options["motors"][motor_joint_name]["effortLimit"] = MAX_TORQUE robot.set_options(robot_options) # ##################### Define some problem-specific variables ######################### if not self.continuous: ## Map between discrete actions and actual motor torque self.AVAIL_TORQUE = [-MAX_TORQUE, MAX_TORQUE] ## Angle at which to fail the episode self.theta_threshold_radians = 25 * pi / 180 ## Position at which to fail the episode self.x_threshold = 0.75 # Internal parameters use to generate sample goals and compute the terminal condition self._tipIdx = robot.pinocchio_model.getFrameId("SecondPendulumMass") self._tipPosZMax = robot.pinocchio_data.oMf[ self._tipIdx].translation[2] # Bounds of the hypercube associated with the initial state of the robot self.state_random_high = np.array([0.2 - pi, 0.2, 1.0, 1.0]) self.state_random_low = np.array([-0.2 - pi, -0.2, -1.0, -1.0]) # ####################### Configure the learning environment ########################### super().__init__("acrobot", engine_py, DT)
def __init__(self, continuous=True): """ @brief Constructor @param[in] continuous Whether the action space is continuous or not. 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. """ ## @var steps_beyond_done # @copydoc RobotJiminyGoalEnv::steps_beyond_done ## @var state # @copydoc RobotJiminyGoalEnv::state ## @var action_space # @copydoc RobotJiminyGoalEnv::action_space ## @var observation_space # @copydoc RobotJiminyGoalEnv::observation_space ## @var state_random_high # @copydoc RobotJiminyGoalEnv::state_random_high ## @var state_random_low # @copydoc RobotJiminyGoalEnv::state_random_low # ########################## Backup the input arguments ################################ ## Flag to determine if the action space is continuous self.continuous = continuous # ############################### Initialize Jiminy #################################### os.environ["JIMINY_MESH_PATH"] = resource_filename( 'gym_jiminy.envs', 'data') urdf_path = os.path.join(os.environ["JIMINY_MESH_PATH"], "double_pendulum/double_pendulum.urdf") self._model = jiminy.Model( ) # Model has to be an attribute of the class to avoid being garbage collected self._model.initialize(urdf_path) motor_joint_names = ("SecondPendulumJoint", ) encoder_joint_names = ("PendulumJoint", "SecondPendulumJoint") for joint_name in motor_joint_names: motor = jiminy.SimpleMotor(joint_name) self._model.attach_motor(motor) motor.initialize(joint_name) for joint_name in encoder_joint_names: encoder = jiminy.EncoderSensor(joint_name) self._model.attach_sensor(encoder) encoder.initialize(joint_name) engine_py = EngineAsynchronous(self._model) # ############################### Configure Jiminy ##################################### model_options = self._model.get_model_options() sensors_options = self._model.get_sensors_options() engine_options = engine_py.get_engine_options() ctrl_options = engine_py.get_controller_options() model_options["telemetry"]["enableEncoderSensors"] = False engine_options["telemetry"]["enableConfiguration"] = False engine_options["telemetry"]["enableVelocity"] = False engine_options["telemetry"]["enableAcceleration"] = False engine_options["telemetry"]["enableTorque"] = False engine_options["telemetry"]["enableEnergy"] = False engine_options["stepper"][ "solver"] = "runge_kutta_dopri5" # ["runge_kutta_dopri5", "explicit_euler"] self._model.set_model_options(model_options) self._model.set_sensors_options(sensors_options) engine_py.set_engine_options(engine_options) engine_py.set_controller_options(ctrl_options) # ##################### Define some problem-specific variables ######################### ## Max velocity of joint 1 self.MAX_VEL_1 = 4 * pi ## Max velocity of joint 2 self.MAX_VEL_2 = 4 * pi if not self.continuous: ## Torque magnitude of the action self.AVAIL_TORQUE = [-1.0, 0.0, +1.0] ## Force mag of the action self.torque_mag = np.array([10.0]) ## Noise standard deviation added to the action self.torque_noise_max = 0.0 ## Angle at which to fail the episode self.theta_threshold_radians = 25 * pi / 180 ## Position at which to fail the episode self.x_threshold = 0.75 # Internal parameters to generate sample goals and compute the terminal condition self._tipIdx = engine_py._engine.model.pinocchio_model.getFrameId( "SecondPendulumMass") self._tipPosZMax = engine_py._engine.model.pinocchio_data.oMf[ self._tipIdx].translation[2] # ####################### Configure the learning environment ########################### # The time step of the 'step' method dt = 2.0e-3 super(JiminyAcrobotGoalEnv, self).__init__("acrobot", engine_py, dt) # #################### Overwrite some problem-generic variables ######################## # Update the velocity bounds of the model model_options = self._model.get_model_options() model_options["joints"]["velocityLimit"] = [ self.MAX_VEL_1, self.MAX_VEL_2 ] model_options["joints"]["velocityLimitFromUrdf"] = False self._model.set_model_options(model_options) # Update the goal spaces and the observation space (which is different from the state space in this case) goal_high = np.array([self._tipPosZMax]) obs_high = np.array( [1.0, 1.0, 1.0, 1.0, self.MAX_VEL_1, self.MAX_VEL_2]) self.observation_space = spaces.Dict( dict(desired_goal=spaces.Box(low=-goal_high, high=goal_high, dtype=np.float64), achieved_goal=spaces.Box(low=-goal_high, high=goal_high, dtype=np.float64), observation=spaces.Box(low=-obs_high, high=obs_high, dtype=np.float64))) self.state_random_high = np.array([0.2 - pi, 0.2, 1.0, 1.0]) self.state_random_low = np.array([-0.2 - pi, -0.2, -1.0, -1.0]) if self.continuous: self.action_space = spaces.Box(low=-self.torque_mag, high=self.torque_mag, dtype=np.float64) else: self.action_space = spaces.Discrete(3)