Add pendulum environment example (forgot in previous commit) + update READMEs

examples/README.rst

@@ -15,7 +15,9 @@ You can check the following folders on:
 - ``kinematics``: how to use forward and inverse kinematics as well as position and velocity control.
 - ``dynamics``: how to use forward and inverse dynamics as well as force control.
 - ``manipulability``: how to use the velocity and dynamic manipulability ellipsoids.
-- ``states``: how to query the states / observations.
 - ``models``: the different learning models that you can use.
+- ``states``: how to query the states / observations.
+- ``rewards``: how to use the reward functions.
+- ``environments``: provide a full example on how to create an environment from scratch in PRL.
 - ``imitation``: how to use imitation learning with the framework.
 - ``gym/cartpole``: policies that are trained with different algorithms on the gym Cartpole environment.

examples/environments/README.rst

@@ -84,8 +84,7 @@ Examples
 
 Here are few examples that you can find in this folder that better demonstrate how to use the environment:
 
-1. ``basics.py``: show the flexibility of how to build an environment and use it.
-2. ``manipulator.py``: show how to define an environment where the goal is to reach a target object using a manipulator.
+1. ``inverted_pendulum.py``: create the inverted pendulum environment from scratch in PRL. This example combines the various concepts that we have seen until now (simulator, world, robot, state, action, reward, physics randomizer, initial state generator, etc). At the end, the environment is launched in a similar way as in OpenAI Gym.
 
 References:
 

examples/environments/inverted_pendulum.py

@@ -0,0 +1,84 @@
+#!/usr/bin/env python
+"""In this file, we create from scratch the inverted pendulum swing-up environment defined in OpenAI Gym.
+
+This is based on the control problem proposed in OpenAI Gym [1]:
+"The inverted pendulum swingup problem is a classic problem in the control literature. In this version of the problem,
+the pendulum starts in a random position, and the goal is to swing it up so it stays upright." [1]
+
+References:
+    - [1] Pendulum environment in OpenAI Gym: https://gym.openai.com/envs/Pendulum-v0/
+"""
+
+import numpy as np
+
+import pyrobolearn as prl
+
+
+# create simulator
+sim = prl.simulators.Bullet()
+
+
+# create basic world with the pendulum
+world = prl.worlds.BasicWorld(sim)
+robot = world.load_robot('pendulum')
+robot.disable_motor()  # such that it swings freely
+robot.print_info()
+
+
+# create state: [cos(q_1), sin(q_1), \dot{q}_1]
+trig_position_state = prl.states.JointTrigonometricPositionState(robot=robot)
+velocity_state = prl.states.JointVelocityState(robot=robot)
+state = trig_position_state + velocity_state
+print("\nObservation: {}".format(state))
+
+
+# create action
+action = prl.actions.JointTorqueAction(robot, f_min=-2., f_max=2.)
+print("\nAction: {}".format(action))
+
+
+# create reward/cost
+position_cost = prl.rewards.JointPositionCost(prl.states.JointPositionState(robot),
+                                              target_state=np.zeros(len(robot.joints)),
+                                              update_state=True)
+velocity_cost = prl.rewards.JointVelocityCost(velocity_state)
+torque_cost = prl.rewards.JointTorqueCost(prl.states.JointForceTorqueState(robot=robot), update_state=True)
+reward = position_cost + 0.1 * velocity_cost + 0.001 * torque_cost
+print("Reward: {}".format(reward))
+
+
+# create initial state generator: generate the state each time we reset the environment
+def reset_robot(robot):  # function to disable the motors every time we reset the joint state
+    def reset():
+        robot.disable_motor()
+    return reset
+
+
+init_state = prl.states.JointPositionState(robot)
+low, high = np.array([-np.pi] * len(robot.joints)), np.array([np.pi] * len(robot.joints))
+initial_state_generator = prl.states.generators.UniformStateGenerator(state=init_state, low=low, high=high,
+                                                                      fct=reset_robot(robot))
+
+
+# create physics randomizer: randomize the mass each time we reset the environment
+masses = robot.get_link_masses(link_ids=robot.joints)
+masses = (masses - masses/10., masses + masses/10.)
+physics_randomizer = prl.physics.LinkPhysicsRandomizer(robot, link_ids=robot.joints, masses=masses)
+
+
+# create the environment using composition
+env = prl.envs.Env(world=world, states=state, rewards=reward, actions=action,
+                   initial_state_generators=initial_state_generator, physics_randomizers=physics_randomizer,
+                   terminal_conditions=None)
+
+
+# run simulation
+env.reset()
+for t in prl.count():
+
+    if (t % 800) == 0:  # reset to see what initial_state_generator and physics randomizer do
+        env.reset()
+        print("New link mass: {}".format(robot.get_link_masses(link_ids=robot.joints)))
+
+    states, rewards, done, info = env.step(sleep_dt=1./240)
+    print("Reward: {}".format(rewards))