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))