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)