From 1beb9ec7893649c303780ad34506fe58bdfe8fb0 Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Thu, 22 Aug 2019 13:06:46 +0200 Subject: [PATCH] update envs --- examples/simulators/bullet_ros_publisher.py | 39 +++- examples/simulators/bullet_ros_subscriber.py | 43 ++++- pyrobolearn/envs/env.py | 15 +- pyrobolearn/envs/locomotion/README.rst | 1 + .../envs/locomotion/robust_quadruped.py | 3 +- pyrobolearn/envs/manipulation/README.rst | 4 + pyrobolearn/envs/sports/README.rst | 6 + pyrobolearn/envs/sports/__init__.py | 0 pyrobolearn/envs/sports/baseball.py | 97 ++++++++++ pyrobolearn/envs/sports/basketball.py | 98 ++++++++++ pyrobolearn/envs/sports/billiard.py | 97 ++++++++++ pyrobolearn/envs/sports/darts.py | 97 ++++++++++ pyrobolearn/envs/sports/football.py | 91 +++++++++ pyrobolearn/envs/sports/kendo.py | 110 +++++++++++ pyrobolearn/envs/sports/ping_pong.py | 175 ++++++++++++++++++ pyrobolearn/envs/warehouse/README.rst | 18 ++ pyrobolearn/envs/warehouse/__init__.py | 0 .../envs/warehouse/move_on_conveyor.py | 92 +++++++++ .../envs/warehouse/regroup_on_conveyor.py | 93 ++++++++++ .../envs/warehouse/rotate_on_conveyor.py | 93 ++++++++++ 20 files changed, 1156 insertions(+), 16 deletions(-) create mode 100644 pyrobolearn/envs/sports/README.rst create mode 100644 pyrobolearn/envs/sports/__init__.py create mode 100644 pyrobolearn/envs/sports/baseball.py create mode 100644 pyrobolearn/envs/sports/basketball.py create mode 100644 pyrobolearn/envs/sports/billiard.py create mode 100644 pyrobolearn/envs/sports/darts.py create mode 100644 pyrobolearn/envs/sports/football.py create mode 100644 pyrobolearn/envs/sports/kendo.py create mode 100644 pyrobolearn/envs/sports/ping_pong.py create mode 100644 pyrobolearn/envs/warehouse/README.rst create mode 100644 pyrobolearn/envs/warehouse/__init__.py create mode 100644 pyrobolearn/envs/warehouse/move_on_conveyor.py create mode 100644 pyrobolearn/envs/warehouse/regroup_on_conveyor.py create mode 100644 pyrobolearn/envs/warehouse/rotate_on_conveyor.py diff --git a/examples/simulators/bullet_ros_publisher.py b/examples/simulators/bullet_ros_publisher.py index 8a911f4..851f8bb 100644 --- a/examples/simulators/bullet_ros_publisher.py +++ b/examples/simulators/bullet_ros_publisher.py @@ -47,14 +47,13 @@ import pyrobolearn as prl # create simulator (ros core will automatically be launched if it has not already been launched) -sim = prl.simulators.BulletROS(subscribe=False, publish=True, teleoperate=True) +sim = prl.simulators.BulletROS(publish=True, teleoperate=True) # load world world = prl.worlds.BasicWorld(sim) -# load rrbot -robot = prl.robots.RRBot(sim) - +# load robot +robot = world.load_robot('wam') # run simulation for t in count(): @@ -64,3 +63,35 @@ for t in count(): # perform a step in the simulator (and sleep for `sim.dt`) world.step(sim.dt) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/simulators/bullet_ros_subscriber.py b/examples/simulators/bullet_ros_subscriber.py index d5e6683..ceb5978 100644 --- a/examples/simulators/bullet_ros_subscriber.py +++ b/examples/simulators/bullet_ros_subscriber.py @@ -15,14 +15,13 @@ import pyrobolearn as prl # create simulator (ros core will automatically be launched if it has not already been launched) -sim = prl.simulators.BulletROS(subscribe=True, publish=False) +sim = prl.simulators.BulletROS(subscribe=True) # load world world = prl.worlds.BasicWorld(sim) -# load rrbot -robot = prl.robots.RRBot(sim) - +# load robot +robot = world.load_robot('wam') # run simulation for t in count(): @@ -31,3 +30,39 @@ for t in count(): # perform a step in the world, and sleep for `sim.dt` world.step(sim.dt) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/pyrobolearn/envs/env.py b/pyrobolearn/envs/env.py index 359889d..43da9ab 100644 --- a/pyrobolearn/envs/env.py +++ b/pyrobolearn/envs/env.py @@ -355,15 +355,16 @@ class Env(gym.Env): # TODO: make it inheriting the gym.Env for randomizer in self.physics_randomizers: randomizer.randomize() - # generate initial states + # generate initial states (states are reset by the states generators) for generator in self.state_generators: - generator(reset_state=False) + generator() # reset_state=False) - self.world.step() + # self.world.step() # reset states and return first states/observations - states = [state.reset(merged_data=True) for state in self.states] - print("Reset: ", states) + # states = [state.reset(merged_data=True) for state in self.states] + # print("Reset: ", states) + states = [state.merged_data for state in self.states] return self._convert_state_to_data(states) def step(self, actions=None, sleep_dt=None): @@ -386,8 +387,8 @@ class Env(gym.Env): # TODO: make it inheriting the gym.Env Returns: observation (object): agent's observation of the current environment reward (float) : amount of reward returned after previous action - done (boolean): whether the episode has ended, in which case further step() calls will return undefined - results + done (bool): whether the episode has ended, in which case further step() calls will return undefined + results info (dict): contains auxiliary diagnostic information (helpful for debugging, and sometimes learning) """ # if not isinstance(actions, (list, tuple)): diff --git a/pyrobolearn/envs/locomotion/README.rst b/pyrobolearn/envs/locomotion/README.rst index 1386ebc..4e71a98 100644 --- a/pyrobolearn/envs/locomotion/README.rst +++ b/pyrobolearn/envs/locomotion/README.rst @@ -1,3 +1,4 @@ Locomotion Environments ----------------------- +This folder contains locomotion environments. diff --git a/pyrobolearn/envs/locomotion/robust_quadruped.py b/pyrobolearn/envs/locomotion/robust_quadruped.py index 071809d..61ea72d 100644 --- a/pyrobolearn/envs/locomotion/robust_quadruped.py +++ b/pyrobolearn/envs/locomotion/robust_quadruped.py @@ -133,10 +133,11 @@ class SelfRightingEnv(LocomotionEnv): action = prl.actions.JointPositionAction(robot, kp=robot.kp, kd=robot.kd) # create state + ang_vel_state = prl.states.BaseAngularVelocityState(robot) q_state = prl.states.JointPositionState(robot) dq_state = prl.states.JointVelocityState(robot) action_state = prl.states.PreviousActionState(action) - state = None + state = ang_vel_state + q_state + dq_state # create cost c_tau = prl.rewards.JointTorqueCost(state=robot) diff --git a/pyrobolearn/envs/manipulation/README.rst b/pyrobolearn/envs/manipulation/README.rst index e69de29..58686eb 100644 --- a/pyrobolearn/envs/manipulation/README.rst +++ b/pyrobolearn/envs/manipulation/README.rst @@ -0,0 +1,4 @@ +Manipulation Environments +------------------------- + +This folder provides manipulation environments. diff --git a/pyrobolearn/envs/sports/README.rst b/pyrobolearn/envs/sports/README.rst new file mode 100644 index 0000000..d73093b --- /dev/null +++ b/pyrobolearn/envs/sports/README.rst @@ -0,0 +1,6 @@ +Sport Environments +================== + +WARNING: This is currently a work in progress. The rewards and other functions have not been defined yet. + +This folder provides sport environments. diff --git a/pyrobolearn/envs/sports/__init__.py b/pyrobolearn/envs/sports/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/envs/sports/baseball.py b/pyrobolearn/envs/sports/baseball.py new file mode 100644 index 0000000..8a97086 --- /dev/null +++ b/pyrobolearn/envs/sports/baseball.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python +r"""Provide the baseball environment. +""" + +# TODO: finish to implement this environment + +import pyrobolearn as prl +from pyrobolearn.worlds.samples.sports.baseball import BaseballWorld +from pyrobolearn.envs.env import Env + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class BaseballEnv(Env): + r"""Baseball environment + """ + + def __init__(self, simulator, robot='kuka_iiwa', verbose=False): + """ + Initialize the baseball environment. + + Args: + simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator. + robot (str): robot name. + verbose (bool): if True, it will print information when creating the environment. + """ + # check simulator + if simulator is None: + simulator = prl.simulators.Bullet() + elif not isinstance(simulator, prl.simulators.Simulator): + raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: " + "{}".format(type(simulator))) + + # create world + world = BaseballWorld(simulator) + + # load manipulator in world + self.robot = world.load_robot(robot) + if not isinstance(self.robot, prl.robots.Manipulator): + raise TypeError("Expecting a manipulator, but got instead {}".format(type(self.robot))) + if verbose: + self.robot.print_info() + + # attach bat to robot end effector + world.attach(body1=self.robot, body2=world.bat, link1=self.robot.end_effectors[0], link2=-1, + joint_axis=[0., 0., 0.], parent_frame_position=[0., 0., world.bat_grip_radius], + child_frame_position=[0., 0.3, 0.], parent_frame_orientation=[0., 0., 0., 1.]) + + # apply force to ball to throw it; f=dp/dt thus dp = f dt (change of momentum) + + # create state + q_state = prl.states.JointPositionState(robot=self.robot) + dq_state = prl.states.JointVelocityState(robot=self.robot) + state = q_state + dq_state + if verbose: + print(state) + + # create action + action = prl.actions.JointPositionAction(robot=self.robot, kp=self.robot.kp, kd=self.robot.kd) + if verbose: + print(action) + + # create reward # TODO: use geometrical rewards + reward = None + + # create terminal condition + terminal_condition = None + + # create initial state generator + initial_state_generator = None + + super(BaseballEnv, self).__init__(world=world, states=state, rewards=reward, actions=action, + terminal_conditions=terminal_condition, + initial_state_generators=initial_state_generator) + + +# Test +if __name__ == '__main__': + from itertools import count + + # create simulator + sim = prl.simulators.Bullet() + + # create environment + env = BaseballEnv(sim) + + # run simulation + for t in count(): + env.step(sleep_dt=sim.dt) diff --git a/pyrobolearn/envs/sports/basketball.py b/pyrobolearn/envs/sports/basketball.py new file mode 100644 index 0000000..0449a0d --- /dev/null +++ b/pyrobolearn/envs/sports/basketball.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python +r"""Provide the basketball environment. +""" + +# TODO: finish to implement this environment + +import pyrobolearn as prl +from pyrobolearn.worlds.samples.sports.basketball import BasketBallWorld +from pyrobolearn.envs.env import Env + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class BasketBallEnv(Env): + r"""Basketball environment + + """ + + def __init__(self, simulator, robot='kuka_iiwa', verbose=False): + """ + Initialize the basketball environment. + + Args: + simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator. + robot (str): robot name. + verbose (bool): if True, it will print information when creating the environment. + """ + # check simulator + if simulator is None: + simulator = prl.simulators.Bullet() + elif not isinstance(simulator, prl.simulators.Simulator): + raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: " + "{}".format(type(simulator))) + + # create world + world = BasketBallWorld(simulator) + + # load manipulator in world + self.robot = world.load_robot(robot) + if not isinstance(self.robot, prl.robots.Manipulator): + raise TypeError("Expecting a manipulator, but got instead {}".format(type(self.robot))) + if verbose: + self.robot.print_info() + + # attach ball to robot end effector + world.attach(body1=self.robot, body2=world.ball, link1=self.robot.end_effectors[0], link2=-1, + joint_axis=[0., 0., 0.], parent_frame_position=[0., 0., world.ball_radius], + child_frame_position=[0., 0., -0.01], parent_frame_orientation=[0., 0., 0., 1.]) + + # apply force to ball to throw it; f=dp/dt thus dp = f dt (change of momentum) + + # create state + q_state = prl.states.JointPositionState(robot=self.robot) + dq_state = prl.states.JointVelocityState(robot=self.robot) + state = q_state + dq_state + if verbose: + print(state) + + # create action + action = prl.actions.JointPositionAction(robot=self.robot, kp=self.robot.kp, kd=self.robot.kd) + if verbose: + print(action) + + # create reward # TODO: use geometrical rewards + reward = None + + # create terminal condition + terminal_condition = None + + # create initial state generator + initial_state_generator = None + + super(BasketBallEnv, self).__init__(world=world, states=state, rewards=reward, actions=action, + terminal_conditions=terminal_condition, + initial_state_generators=initial_state_generator) + + +# Test +if __name__ == '__main__': + from itertools import count + + # create simulator + sim = prl.simulators.Bullet() + + # create environment + env = BasketBallEnv(sim) + + # run simulation + for t in count(): + env.step(sleep_dt=sim.dt) diff --git a/pyrobolearn/envs/sports/billiard.py b/pyrobolearn/envs/sports/billiard.py new file mode 100644 index 0000000..f0754d2 --- /dev/null +++ b/pyrobolearn/envs/sports/billiard.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python +r"""Provide the billiard environment. +""" + +# TODO: finish to implement this environment + +import pyrobolearn as prl +from pyrobolearn.worlds.samples.sports.billiard import BilliardWorld +from pyrobolearn.envs.env import Env + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class BilliardEnv(Env): + r"""Billiard environment + + """ + + def __init__(self, simulator, robot='kuka_iiwa', verbose=False): + """ + Initialize the billiard environment. + + Args: + simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator. + robot (str): robot name. + verbose (bool): if True, it will print information when creating the environment. + """ + # check simulator + if simulator is None: + simulator = prl.simulators.Bullet() + elif not isinstance(simulator, prl.simulators.Simulator): + raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: " + "{}".format(type(simulator))) + + # create world + world = BilliardWorld(simulator) + + # load manipulator in world + self.robot = world.load_robot(robot, position=[-2., 0.2, 0.]) + if not isinstance(self.robot, prl.robots.Manipulator): + raise TypeError("Expecting a manipulator, but got instead {}".format(type(self.robot))) + if verbose: + self.robot.print_info() + + # attach cue to robot end effector + # Note that you can detach the cue from the robot end effector using `world.detach` + world.attach(body1=self.robot, body2=world.cue1, link1=self.robot.end_effectors[0], link2=-1, + joint_axis=[0., 0., 0.], parent_frame_position=[-0., 0., 0.02], child_frame_position=[0., 0., 0.], + parent_frame_orientation=[0., 0., 0., 1.]) + + # create state + q_state = prl.states.JointPositionState(robot=self.robot) + dq_state = prl.states.JointVelocityState(robot=self.robot) + state = q_state + dq_state + if verbose: + print(state) + + # create action + action = prl.actions.JointPositionAction(robot=self.robot, kp=self.robot.kp, kd=self.robot.kd) + if verbose: + print(action) + + # create reward # TODO: use geometrical rewards + reward = None + + # create terminal condition + terminal_condition = None + + # create initial state generator + initial_state_generator = None + + super(BilliardEnv, self).__init__(world=world, states=state, rewards=reward, actions=action, + terminal_conditions=terminal_condition, + initial_state_generators=initial_state_generator) + + +# Test +if __name__ == '__main__': + from itertools import count + + # create simulator + sim = prl.simulators.Bullet() + + # create world + env = BilliardEnv(sim) + + # run simulation + for t in count(): + env.step(sleep_dt=sim.dt) diff --git a/pyrobolearn/envs/sports/darts.py b/pyrobolearn/envs/sports/darts.py new file mode 100644 index 0000000..2e6089d --- /dev/null +++ b/pyrobolearn/envs/sports/darts.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python +r"""Provide the darts environments. +""" + +# TODO: finish to implement this environment + +import pyrobolearn as prl +from pyrobolearn.worlds.samples.sports.darts import DartsWorld +from pyrobolearn.envs.env import Env + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class DartsEnv(Env): + r"""Darts environment + + """ + + def __init__(self, simulator, robot='kuka_iiwa', verbose=False): + """ + Initialize the darts environment. + + Args: + simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator. + robot (str): robot name. + verbose (bool): if True, it will print information when creating the environment. + """ + # check simulator + if simulator is None: + simulator = prl.simulators.Bullet() + elif not isinstance(simulator, prl.simulators.Simulator): + raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: " + "{}".format(type(simulator))) + + # create world + world = DartsWorld(simulator) + + # load manipulator in world + self.robot = world.load_robot(robot) + if not isinstance(self.robot, prl.robots.Manipulator): + raise TypeError("Expecting a manipulator, but got instead {}".format(type(self.robot))) + if verbose: + self.robot.print_info() + + # attach first dart to robot end effector + world.attach(body1=self.robot, body2=world.darts[0], link1=self.robot.end_effectors[0], link2=-1, + joint_axis=[0., 0., 0.], + parent_frame_position=[0., 0., 0.02], child_frame_position=[0., 0., 0.], + parent_frame_orientation=[0, 0., 0., 1.]) + + # create state + q_state = prl.states.JointPositionState(robot=self.robot) + dq_state = prl.states.JointVelocityState(robot=self.robot) + state = q_state + dq_state + if verbose: + print(state) + + # create action + action = prl.actions.JointPositionAction(robot=self.robot, kp=self.robot.kp, kd=self.robot.kd) + if verbose: + print(action) + + # create reward # TODO: use geometrical rewards + reward = None + + # create terminal condition + terminal_condition = None + + # create initial state generator + initial_state_generator = None + + super(DartsEnv, self).__init__(world=world, states=state, rewards=reward, actions=action, + terminal_conditions=terminal_condition, + initial_state_generators=initial_state_generator) + + +# Test +if __name__ == '__main__': + from itertools import count + + # create simulator + sim = prl.simulators.Bullet() + + # create environment + env = DartsEnv(sim) + + # run simulation + for t in count(): + env.step(sleep_dt=sim.dt) diff --git a/pyrobolearn/envs/sports/football.py b/pyrobolearn/envs/sports/football.py new file mode 100644 index 0000000..f9c4a74 --- /dev/null +++ b/pyrobolearn/envs/sports/football.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python +r"""Provide the football/soccer environment. +""" + +# TODO: finish to implement this environment + +import pyrobolearn as prl +from pyrobolearn.worlds.samples.sports.football import FootballWorld +from pyrobolearn.envs.env import Env + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class FootballEnv(Env): + r"""Football/Soccer environment. + + """ + + def __init__(self, simulator, robot='coman', verbose=False): + """ + Initialize the darts environment. + + Args: + simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator. + robot (str): robot name. + verbose (bool): if True, it will print information when creating the environment. + """ + # check simulator + if simulator is None: + simulator = prl.simulators.Bullet() + elif not isinstance(simulator, prl.simulators.Simulator): + raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: " + "{}".format(type(simulator))) + + # create world + world = FootballWorld(simulator) + + # load manipulator in world + self.robot = world.load_robot(robot) + if not isinstance(self.robot, prl.robots.LeggedRobot): + raise TypeError("Expecting a manipulator, but got instead {}".format(type(self.robot))) + if verbose: + self.robot.print_info() + + # create state + q_state = prl.states.JointPositionState(robot=self.robot) + dq_state = prl.states.JointVelocityState(robot=self.robot) + state = q_state + dq_state + if verbose: + print(state) + + # create action + action = prl.actions.JointPositionAction(robot=self.robot, kp=self.robot.kp, kd=self.robot.kd) + if verbose: + print(action) + + # create reward # TODO: use geometrical rewards + reward = None + + # create terminal condition + terminal_condition = None + + # create initial state generator + initial_state_generator = None + + super(FootballEnv, self).__init__(world=world, states=state, rewards=reward, actions=action, + terminal_conditions=terminal_condition, + initial_state_generators=initial_state_generator) + + +# Test +if __name__ == '__main__': + from itertools import count + + # create simulator + sim = prl.simulators.Bullet() + + # create environment + env = FootballEnv(sim, robot='coman') + + # run simulation + for t in count(): + env.step(sleep_dt=sim.dt) diff --git a/pyrobolearn/envs/sports/kendo.py b/pyrobolearn/envs/sports/kendo.py new file mode 100644 index 0000000..57405da --- /dev/null +++ b/pyrobolearn/envs/sports/kendo.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python +r"""Provide the kendo environment. +""" + +# TODO: finish to implement this environment + +import pyrobolearn as prl +from pyrobolearn.worlds.samples.sports.kendo import KendoWorld +from pyrobolearn.envs.env import Env + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class KendoEnv(Env): + r"""Kendo environment. + + """ + + def __init__(self, simulator, robot='kuka_iiwa', verbose=False): + """ + Initialize the kendo environment. + + Args: + simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator. + robot (str): robot name. + verbose (bool): if True, it will print information when creating the environment. + """ + # check simulator + if simulator is None: + simulator = prl.simulators.Bullet() + elif not isinstance(simulator, prl.simulators.Simulator): + raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: " + "{}".format(type(simulator))) + + # create world + world = KendoWorld(simulator, position=(0., 0., 1.5), num_shinai=2) + + # create manipulators + robot1 = world.load_robot(robot) + robot2 = world.load_robot(robot, position=(1., 0.), orientation=(0., 0., 1., 0.)) + self.robot1, self.robot2 = robot1, robot2 + + # attach shinai to robot end effectors + world.attach(body1=robot1, body2=world.shinai[0], link1=robot1.end_effectors[0], link2=-1, + joint_axis=[0., 0., 0.], + parent_frame_position=[0., 0., 0.02], child_frame_position=[0., 0., 0.15], + parent_frame_orientation=[0, -0.707, 0., .707]) + world.attach(body1=robot2, body2=world.shinai[1], link1=robot2.end_effectors[0], link2=-1, + joint_axis=[0., 0., 0.], + parent_frame_position=[0., 0., 0.02], child_frame_position=[0., 0., 0.15], + parent_frame_orientation=[0, -0.707, 0., .707]) + + if not isinstance(robot1, prl.robots.Manipulator): + raise TypeError("Expecting a manipulator, but got instead {}".format(type(robot1))) + if verbose: + self.robot1.print_info() + + # create states + states = [] + for i, robot in enumerate([robot1, robot2]): + q_state = prl.states.JointPositionState(robot=robot) + dq_state = prl.states.JointVelocityState(robot=robot) + state = q_state + dq_state + states.append(state) + if verbose: + print(states) + + # create actions + actions = [] + for i, robot in enumerate([robot1, robot2]): + action = prl.actions.JointPositionAction(robot=robot, kp=robot.kp, kd=robot.kd) + actions.append(action) + if verbose: + print(actions) + + # create reward # TODO: use geometrical rewards + reward = None + + # create terminal condition + terminal_condition = None + + # create initial state generator + initial_state_generator = None + + super(KendoEnv, self).__init__(world=world, states=states, rewards=reward, actions=actions, + terminal_conditions=terminal_condition, + initial_state_generators=initial_state_generator) + + +# Test +if __name__ == '__main__': + from itertools import count + + # create simulator + sim = prl.simulators.Bullet() + + # create environment + env = KendoEnv(sim) + + # run simulation + for t in count(): + env.step(sleep_dt=sim.dt) diff --git a/pyrobolearn/envs/sports/ping_pong.py b/pyrobolearn/envs/sports/ping_pong.py new file mode 100644 index 0000000..b298a1c --- /dev/null +++ b/pyrobolearn/envs/sports/ping_pong.py @@ -0,0 +1,175 @@ +#!/usr/bin/env python +r"""Provide the ping pong (table tennis) environment. +""" + +# TODO: finish to implement this environment + +import pyrobolearn as prl +from pyrobolearn.worlds.samples.sports.ping_pong import PingPongWorld, BallOnPaddleWorld +from pyrobolearn.envs.env import Env + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class PingPongEnv(Env): + r"""Ping Pong environment. + + """ + + def __init__(self, simulator, robot='kuka_iiwa', verbose=False): + """ + Initialize the ping pong environment. + + Args: + simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator. + robot (str): robot name. + verbose (bool): if True, it will print information when creating the environment. + """ + # check simulator + if simulator is None: + simulator = prl.simulators.Bullet() + elif not isinstance(simulator, prl.simulators.Simulator): + raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: " + "{}".format(type(simulator))) + + # create world + world = PingPongWorld(simulator) + + # load 2 manipulators in world + robot1 = world.load_robot(robot, position=[1.8, 0., 0.2], fixed_base=True) + robot2 = world.load_robot(robot, position=[-1.8, 0., 0.2], fixed_base=True) + self.robot1, self.robot2 = robot1, robot2 + if not isinstance(robot1, prl.robots.Manipulator): + raise TypeError("Expecting a manipulator, but got instead {}".format(type(robot1))) + + # attach each paddle to the robot's end-effector + world.attach(body1=robot1, body2=world.paddle1, link1=robot1.end_effectors[0], link2=-1, + joint_axis=[0., 0., 0.], + parent_frame_position=[0., 0., 0.02], child_frame_position=[0., 0., 0.], + parent_frame_orientation=[0, -0.707, 0, 0.707]) + world.attach(body1=robot2, body2=world.paddle2, link1=robot2.end_effectors[0], link2=-1, + joint_axis=[0., 0., 0.], + parent_frame_position=[0., 0., 0.02], child_frame_position=[0., 0., 0.], + parent_frame_orientation=[0, 0.707, 0, 0.707]) + + if verbose: + self.robot1.print_info() + + # create states + states = [] + for i, robot in enumerate([robot1, robot2]): + q_state = prl.states.JointPositionState(robot=robot) + dq_state = prl.states.JointVelocityState(robot=robot) + state = q_state + dq_state + states.append(state) + if verbose: + print(states) + + # create actions + actions = [] + for i, robot in enumerate([robot1, robot2]): + action = prl.actions.JointPositionAction(robot=robot, kp=robot.kp, kd=robot.kd) + actions.append(action) + if verbose: + print(actions) + + # create reward # TODO: use geometrical rewards + reward = None + + # create terminal condition + terminal_condition = None + + # create initial state generator + initial_state_generator = None + + super(PingPongEnv, self).__init__(world=world, states=states, rewards=reward, actions=actions, + terminal_conditions=terminal_condition, + initial_state_generators=initial_state_generator) + + +class BallOnPaddleEnv(Env): + r"""Ball on a paddle environment. + + """ + + def __init__(self, simulator, robot='kuka_iiwa', verbose=False): + """ + Initialize the ball on paddle environment. + + Args: + simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator. + robot (str): robot name. + verbose (bool): if True, it will print information when creating the environment. + """ + # check simulator + if simulator is None: + simulator = prl.simulators.Bullet() + elif not isinstance(simulator, prl.simulators.Simulator): + raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: " + "{}".format(type(simulator))) + + # create world + world = BallOnPaddleWorld(simulator) + + # load manipulator in world + self.robot = world.load_robot(robot) + if not isinstance(self.robot, prl.robots.Manipulator): + raise TypeError("Expecting a manipulator, but got instead {}".format(type(self.robot))) + if verbose: + self.robot.print_info() + + # attach each paddle to the robot's end-effector + world.attach(body1=self.robot, body2=world.paddle, link1=self.robot.end_effectors[0], link2=-1, + joint_axis=[0., 0., 0.], parent_frame_position=[0., 0., 0.02], child_frame_position=[0., 0., 0.], + parent_frame_orientation=[0, 0.707, 0, 0.707]) + + world.ball.position = [0., 0., 2.] + + # create state + q_state = prl.states.JointPositionState(robot=self.robot) + dq_state = prl.states.JointVelocityState(robot=self.robot) + state = q_state + dq_state + if verbose: + print(state) + + # create action + action = prl.actions.JointPositionAction(robot=self.robot, kp=self.robot.kp, kd=self.robot.kd) + if verbose: + print(action) + + # create reward # TODO: use geometrical rewards + reward = None + + # create terminal condition + terminal_condition = None + + # create initial state generator + initial_state_generator = None + + super(BallOnPaddleEnv, self).__init__(world=world, states=state, rewards=reward, actions=action, + terminal_conditions=terminal_condition, + initial_state_generators=initial_state_generator) + + +# Test +if __name__ == '__main__': + from itertools import count + + # create simulator + sim = prl.simulators.Bullet() + + # create environment + # env = BallOnPaddleEnv(sim) + env = PingPongEnv(sim) + + # run the simulation + for t in count(): + env.step(sleep_dt=sim.dt) diff --git a/pyrobolearn/envs/warehouse/README.rst b/pyrobolearn/envs/warehouse/README.rst new file mode 100644 index 0000000..9a016cf --- /dev/null +++ b/pyrobolearn/envs/warehouse/README.rst @@ -0,0 +1,18 @@ +Warehouse Environments +====================== + +WARNING: This is currently a work in progress. The rewards and other functions have not been defined yet. + +This folder contains environments where robots have to move objects from one place to another. +This can be carried out by transporting them from one place to another, or picking them up from one place and dropping +them on another place. + +An example of the former is to move a box from one room to another room using a wheeled robot. An example of the latter +is to pick up a box from one belt conveyor using a robot manipulator and drop it on another conveyor (which is close to +the first one). + +Other environments consist: + +- to pick up an object on a belt conveyor and rotate it to a desired rotation before dropping it on the same conveyor +- to pick up scattered objects and regroup them on a belt conveyor, or put them in a specific order / pattern +- to assemble objects together (see the peg-in-hole assembly problem) diff --git a/pyrobolearn/envs/warehouse/__init__.py b/pyrobolearn/envs/warehouse/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/envs/warehouse/move_on_conveyor.py b/pyrobolearn/envs/warehouse/move_on_conveyor.py new file mode 100644 index 0000000..479a162 --- /dev/null +++ b/pyrobolearn/envs/warehouse/move_on_conveyor.py @@ -0,0 +1,92 @@ +#!/usr/bin/env python +r"""Move boxes from one conveyor belt to another one. +""" + +# TODO: finish to implement this environment + +import pyrobolearn as prl +from pyrobolearn.worlds.samples.warehouse.move_on_conveyor import MoveBoxesOnConveyorWorld +from pyrobolearn.envs.env import Env + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class MoveBoxesOnConveyorEnv(Env): + r"""Move boxes on conveyor belt environment. + + This provides an environment where one box has to be moved from one conveyor to another. + """ + + def __init__(self, simulator, robot='kuka_iiwa', verbose=False): + """ + Initialize the environment. + + Args: + simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator. + robot (str): robot name. + verbose (bool): if True, it will print information when creating the environment. + """ + # check simulator + if simulator is None: + simulator = prl.simulators.Bullet() + elif not isinstance(simulator, prl.simulators.Simulator): + raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: " + "{}".format(type(simulator))) + + # create world + world = MoveBoxesOnConveyorWorld(simulator) + + # load manipulator in world + self.robot = world.load_robot(robot, position=(0., -0.3, 0.5)) + if not isinstance(self.robot, prl.robots.Manipulator): + raise TypeError("Expecting a manipulator, but got instead {}".format(type(self.robot))) + if verbose: + self.robot.print_info() + + # create state + q_state = prl.states.JointPositionState(robot=self.robot) + dq_state = prl.states.JointVelocityState(robot=self.robot) + state = q_state + dq_state + if verbose: + print(state) + + # create action + action = prl.actions.JointPositionAction(robot=self.robot, kp=self.robot.kp, kd=self.robot.kd) + if verbose: + print(action) + + # create reward # TODO: use geometrical rewards + reward = None + + # create terminal condition + terminal_condition = None + + # create initial state generator + initial_state_generator = None + + super(MoveBoxesOnConveyorEnv, self).__init__(world=world, states=state, rewards=reward, actions=action, + terminal_conditions=terminal_condition, + initial_state_generators=initial_state_generator) + + +# Test +if __name__ == '__main__': + from itertools import count + + # create simulator + sim = prl.simulators.Bullet() + + # create environment + env = MoveBoxesOnConveyorEnv(sim, robot='kuka_iiwa') + + # run simulation + for t in count(): + env.step(sleep_dt=sim.dt) diff --git a/pyrobolearn/envs/warehouse/regroup_on_conveyor.py b/pyrobolearn/envs/warehouse/regroup_on_conveyor.py new file mode 100644 index 0000000..9bfa2e3 --- /dev/null +++ b/pyrobolearn/envs/warehouse/regroup_on_conveyor.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python +r"""Regroup small boxes that are on a conveyor. +""" + +# TODO: finish to implement this environment + +import pyrobolearn as prl +from pyrobolearn.worlds.samples.warehouse.regroup_on_conveyor import RegroupBoxesOnConveyorWorld +from pyrobolearn.envs.env import Env + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class RegroupBoxesOnConveyorEnv(Env): + r"""Regroup boxes on conveyor belt environment. + + This provides an environment where small boxes that are randomly distributed on the conveyor belt has to be + regrouped to form a certain pattern (like 16 boxes regrouped in a 4x4 grid). + """ + + def __init__(self, simulator, robot='kuka_iiwa', verbose=False): + """ + Initialize the environment. + + Args: + simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator. + robot (str): robot name. + verbose (bool): if True, it will print information when creating the environment. + """ + # check simulator + if simulator is None: + simulator = prl.simulators.Bullet() + elif not isinstance(simulator, prl.simulators.Simulator): + raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: " + "{}".format(type(simulator))) + + # create world + world = RegroupBoxesOnConveyorWorld(simulator) + + # load manipulator in world + self.robot = world.load_robot(robot, position=(0., -0.3, 0.5)) + if not isinstance(self.robot, prl.robots.Manipulator): + raise TypeError("Expecting a manipulator, but got instead {}".format(type(self.robot))) + if verbose: + self.robot.print_info() + + # create state + q_state = prl.states.JointPositionState(robot=self.robot) + dq_state = prl.states.JointVelocityState(robot=self.robot) + state = q_state + dq_state + if verbose: + print(state) + + # create action + action = prl.actions.JointPositionAction(robot=self.robot, kp=self.robot.kp, kd=self.robot.kd) + if verbose: + print(action) + + # create reward + reward = None + + # create terminal condition + terminal_condition = None + + # create initial state generator + initial_state_generator = None + + super(RegroupBoxesOnConveyorEnv, self).__init__(world=world, states=state, rewards=reward, actions=action, + terminal_conditions=terminal_condition, + initial_state_generators=initial_state_generator) + + +# Test +if __name__ == '__main__': + from itertools import count + + # create simulator + sim = prl.simulators.Bullet() + + # create environment + env = RegroupBoxesOnConveyorEnv(sim, robot='kuka_iiwa') + + # run simulation + for t in count(): + env.step(sleep_dt=sim.dt) diff --git a/pyrobolearn/envs/warehouse/rotate_on_conveyor.py b/pyrobolearn/envs/warehouse/rotate_on_conveyor.py new file mode 100644 index 0000000..9af7396 --- /dev/null +++ b/pyrobolearn/envs/warehouse/rotate_on_conveyor.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python +r"""Rotate boxes that are on a conveyor. +""" + +# TODO: finish to implement this environment + +import pyrobolearn as prl +from pyrobolearn.worlds.samples.warehouse.rotate_on_conveyor import RotateBoxesOnConveyorWorld +from pyrobolearn.envs.env import Env + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class RotateBoxesOnConveyorEnv(Env): + r"""Rotate boxes on conveyor belt environment + + This provides an environment where boxes have to be rotated in the correct orientation and put back on the conveyor + belt. + """ + + def __init__(self, simulator, robot='kuka_iiwa', verbose=False): + """ + Initialize the environment. + + Args: + simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator. + robot (str): robot name. + verbose (bool): if True, it will print information when creating the environment. + """ + # check simulator + if simulator is None: + simulator = prl.simulators.Bullet() + elif not isinstance(simulator, prl.simulators.Simulator): + raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: " + "{}".format(type(simulator))) + + # create world + world = RotateBoxesOnConveyorWorld(simulator) + + # load manipulator in world + self.robot = world.load_robot(robot, position=(0., -0.3, 0.5)) + if not isinstance(self.robot, prl.robots.Manipulator): + raise TypeError("Expecting a manipulator, but got instead {}".format(type(self.robot))) + if verbose: + self.robot.print_info() + + # create state + q_state = prl.states.JointPositionState(robot=self.robot) + dq_state = prl.states.JointVelocityState(robot=self.robot) + state = q_state + dq_state + if verbose: + print(state) + + # create action + action = prl.actions.JointPositionAction(robot=self.robot, kp=self.robot.kp, kd=self.robot.kd) + if verbose: + print(action) + + # create reward + reward = None + + # create terminal condition + terminal_condition = None + + # create initial state generator + initial_state_generator = None + + super(RotateBoxesOnConveyorEnv, self).__init__(world=world, states=state, rewards=reward, actions=action, + terminal_conditions=terminal_condition, + initial_state_generators=initial_state_generator) + + +# Test +if __name__ == '__main__': + from itertools import count + + # create simulator + sim = prl.simulators.Bullet() + + # create world + env = RotateBoxesOnConveyorEnv(sim, robot='kuka_iiwa') + + # run simulation + for t in count(): + env.step(sleep_dt=sim.dt)