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 setUp(self):
# Load URDF, create robot.
urdf_path = "data/point_mass.urdf"
# Define the parameters of the contact dynamics
self.k_contact = 1.0e6
self.nu_contact = 2.0e3
self.v_stiction = 5e-2
self.r_stiction = 0.5
self.dry_friction = 5.5
self.visc_friction = 2.0
self.dtMax = 1.0e-5
# Create the jiminy robot and controller
self.robot = jiminy.Robot()
self.robot.initialize(urdf_path, has_freeflyer=True)
self.robot.add_contact_points(['MassBody'])
force_sensor = jiminy.ForceSensor('MassBody')
self.robot.attach_sensor(force_sensor)
force_sensor.initialize('MassBody')
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)
from jiminy_py import core as jiminy
from jiminy_py.viewer import extract_viewer_data_from_log, play_trajectories
# ################################ 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
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 test_constraint_external_force(self):
"""
@brief Test support of external force applied with constraints.
@details To provide a non-trivial test case with an external force
non-colinear to the constraints, simulate two masses
oscillating, one along the x axis and the other along the y
axis, with a spring between them (in diagonal). The system
may be represented as such ((f) indicates fixed bodies)
[M_22 (f)]
^
^
[M_21]\<>\
^ \<>\
^ \<>\
^ \<>\
[O (f)] <><> [M_11] <><> [M_12 (f)]
"""
# Build two robots with freeflyers, with a freeflyer and a fixed second
# body constraint.
# Rebuild the model with a freeflyer
robots = [jiminy.Robot(), jiminy.Robot()]
engine = jiminy.EngineMultiRobot()
# Configure the engine
engine_options = engine.get_options()
engine_options["stepper"]["solver"] = "runge_kutta_dopri5"
engine_options["stepper"]["tolAbs"] = TOLERANCE * 1e-1
engine_options["stepper"]["tolRel"] = TOLERANCE * 1e-1
engine.set_options(engine_options)
# Define some internal parameters
systems_names = ['FirstSystem', 'SecondSystem']
k = np.array([[100, 50], [80, 120]])
nu = np.array([[0.2, 0.01], [0.05, 0.1]])
k_cross = 100
# Initialize and configure the engine
for i in [0, 1]:
# Load robot
robots[i] = load_urdf_default(
self.urdf_name, self.motors_names, has_freeflyer=True)
# Apply constraints
freeflyer_constraint = jiminy.FixedFrameConstraint("world")
robots[i].add_constraint("world", freeflyer_constraint)
fix_mass_constraint = jiminy.FixedFrameConstraint("SecondMass")
robots[i].add_constraint("fixMass", fix_mass_constraint)
# Create controller
class Controller:
def __init__(self, k, nu):
self.k = k
self.nu = nu
def compute_command(self, t, q, v, sensors_data, command):
pass
def internal_dynamics(self, t, q, v, sensors_data, u_custom):
u_custom[6:] = - self.k * q[7:] - self.nu * v[6:]
controller = Controller(k[i, :], nu[i, :])
controller = jiminy.BaseControllerFunctor(
controller.compute_command, controller.internal_dynamics)
controller.initialize(robots[i])
# Add system to engine
engine.add_system(systems_names[i], robots[i], controller)
# Add coupling force
def force(t, q1, v1, q2, v2, f):
# Putting a linear spring between both systems would actually
# decouple them (the force applied on each system would be a
# function of this system state only). So we use a nonlinear
# stiffness, proportional to the square of the distance of both
# systems to the origin.
d2 = q1[7] ** 2 + q2[7] ** 2
f[0] = - k_cross * (1 + d2) * q1[7]
f[1] = + k_cross * (1 + d2) * q2[7]
engine.register_force_coupling(
systems_names[0], systems_names[1], "FirstMass", "FirstMass", force)
# Initialize the whole system.
x_init = {}
## First system: freeflyer at identity
x_init['FirstSystem'] = np.zeros(17)
x_init['FirstSystem'][7:9] = self.x0[:2]
x_init['FirstSystem'][-2:] = self.x0[2:]
x_init['FirstSystem'][6] = 1.0
## Second system: rotation by pi/2 around Z to bring X axis to Y axis
x_init['SecondSystem'] = x_init['FirstSystem'].copy()
x_init['SecondSystem'][5:7] = np.sqrt(2) / 2.0
# Run simulation and extract some information from log data
time, x_jiminy = simulate_and_get_state_evolution(
engine, self.tf, x_init, split=False)
# Verify that both freeflyers didn't moved
for x in x_jiminy:
self.assertTrue(np.allclose(x[:, 9:15], 0.0, atol=TOLERANCE))
self.assertTrue(np.allclose(x[:, :7], x[0, :7], atol=TOLERANCE))
# Extract coordinates in a minimum state vector
x_jiminy_extract = np.hstack([x[:, [7, 8, 15, 16]] for x in x_jiminy])
# Define dynamics of this system
def system_dynamics(t, x):
# Velocity to position
dx = np.zeros(8)
dx[:2] = x[2:4]
dx[4:6] = x[6:8]
# Compute acceleration linked to the spring
for i in range(2):
dx[2 + 4 * i] = (
- k[i, 0] * x[4 * i] - nu[i, 0] * x[2 + 4 * i]
+ k[i, 1] * x[1 + 4 * i] + nu[i, 1] * x[3 + 4 * i])
# Coupling force between both system
dsquared = x[0] ** 2 + x[4] ** 2
dx[2] += - k_cross * (1 + dsquared) * x[0]
dx[6] += - k_cross * (1 + dsquared) * x[4]
for i in [0, 1]:
# Divide forces by mass
dx[2 + 4 * i] /= self.m[0]
# Second mass accelration should be opposite of the first
dx[3 + 4 * i] = -dx[2 + 4 * i]
return dx
x0 = np.hstack([x_init[key][[7, 8, 15, 16]] for key in x_init])
x_python = integrate_dynamics(time, x0, system_dynamics)
self.assertTrue(np.allclose(
x_jiminy_extract, x_python, atol=TOLERANCE))