diff --git a/pyrobolearn/actions/action.py b/pyrobolearn/actions/action.py index 01cec59..3290d8e 100644 --- a/pyrobolearn/actions/action.py +++ b/pyrobolearn/actions/action.py @@ -591,7 +591,10 @@ class Action(object): """ If all the actions are discrete, then it is discrete. """ - return all(self.has_discrete_values()) + values = self.has_discrete_values() + if len(values) == 0: + return False + return all(values) def has_continuous_values(self): """ diff --git a/pyrobolearn/actions/robot_actions/joint_actions.py b/pyrobolearn/actions/robot_actions/joint_actions.py index 4059f52..88e069e 100644 --- a/pyrobolearn/actions/robot_actions/joint_actions.py +++ b/pyrobolearn/actions/robot_actions/joint_actions.py @@ -256,8 +256,9 @@ class JointPositionAction(JointAction): def _write_continuous(self, data): """apply the action data on the robot.""" - self.robot.set_joint_positions(data, self.joints, bounds=self.bounds, kp=self.kp, kd=self.kd, - forces=self.max_force, discrete_values=self.discrete_values) + # self.robot.set_joint_positions(data, self.joints, bounds=self.bounds, kp=self.kp, kd=self.kd, + # forces=self.max_force, discrete_values=self.discrete_values) + self.robot.set_joint_positions(data, self.joints, kp=self.kp, kd=self.kd, forces=self.max_force) def __copy__(self): """Return a shallow copy of the action. This can be overridden in the child class.""" diff --git a/pyrobolearn/robots/__init__.py b/pyrobolearn/robots/__init__.py index 245b201..203c272 100644 --- a/pyrobolearn/robots/__init__.py +++ b/pyrobolearn/robots/__init__.py @@ -35,7 +35,7 @@ from .humanoid import Humanoid from .aibo import Aibo from .minitaur import Minitaur from .littledog import LittleDog -# from .anymal import ANYmal +from .anymal import ANYmal from .hyq import HyQ from .hyq2max import HyQ2Max from .opendog import OpenDog @@ -172,3 +172,12 @@ for robot_name in implemented_robots: implemented_robots = set(list(robot_names_to_classes.keys())) implemented_grippers = set(implemented_grippers) + + +# function to disable the motors +# this can be useful when resetting the joint state +def reset_robot(robot, joint_ids=None): + """Return a function that disables the motors.""" + def reset(): + robot.disable_motor(joint_ids=joint_ids) + return reset diff --git a/pyrobolearn/robots/allegrohand.py b/pyrobolearn/robots/allegrohand.py index 45b9abc..61fa5ba 100644 --- a/pyrobolearn/robots/allegrohand.py +++ b/pyrobolearn/robots/allegrohand.py @@ -24,7 +24,7 @@ class AllegroHand(Hand): - [2] https://github.com/simlabrobotics/allegro_hand_ros """ - def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), scale=1., fixed_base=True): + def __init__(self, simulator, position=(0, 0, 0.1), orientation=(0, 0, 0, 1), scale=1., fixed_base=True): # left=False """ Initialize the Allegro hand. @@ -38,9 +38,9 @@ class AllegroHand(Hand): """ # check parameters if position is None: - position = (0., 0., 0.) + position = (0., 0., 0.1) if len(position) == 2: # assume x, y are given - position = tuple(position) + (0.,) + position = tuple(position) + (0.1,) if orientation is None: orientation = (0, 0, 0, 1) if fixed_base is None: diff --git a/pyrobolearn/states/state.py b/pyrobolearn/states/state.py index 2e43d98..62673dc 100644 --- a/pyrobolearn/states/state.py +++ b/pyrobolearn/states/state.py @@ -741,7 +741,10 @@ class State(object): """ If all the states are discrete, then it is discrete. """ - return all(self.has_discrete_values()) + values = self.has_discrete_values() + if len(values) == 0: + return False + return all(values) def has_continuous_values(self): """ diff --git a/pyrobolearn/tasks/imitation.py b/pyrobolearn/tasks/imitation.py index 8e0529d..3254d9a 100644 --- a/pyrobolearn/tasks/imitation.py +++ b/pyrobolearn/tasks/imitation.py @@ -20,7 +20,7 @@ from pyrobolearn.tools.bridges.mouse_keyboard import BridgeMouseKeyboardImitatio __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "GNU GPLv3" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" @@ -188,10 +188,10 @@ class ILTask(Task): for policy in self.policies: # prev_obs = copy.deepcopy(policy.states.data) if self.testing_enabled: - actions = policy.act(policy.states, deterministic=deterministic) + actions = policy.act(policy.states, deterministic=deterministic, to_numpy=True, apply_action=True) else: actions = None - obs, rew, done, info = self.env.step(actions) + obs, rew, done, info = self.env.step() self._done = done # d = {'prev_obs': prev_obs, 'actions': copy.deepcopy(actions.data), # 'obs': copy.deepcopy(policy.states.data), 'rew': rew, 'done': done} @@ -352,7 +352,7 @@ class ILTask(Task): dt = 1. / 240 # run several steps in the environment - print('Test: resetting...') + # print('Test: resetting...') self.reset() # time.sleep(10) for t in count(): diff --git a/pyrobolearn/tasks/task.py b/pyrobolearn/tasks/task.py index d2c049c..63f03ef 100644 --- a/pyrobolearn/tasks/task.py +++ b/pyrobolearn/tasks/task.py @@ -33,7 +33,7 @@ from pyrobolearn.policies import Policy __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "GNU GPLv3" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" @@ -72,8 +72,9 @@ class Task(object): policies (list of Policy, Policy): the policy(ies). """ # check the environment - if not isinstance(environment, (Env, gym.Env)): - raise TypeError("Expecting 'environment' to be an instance of Env or gym.Env") + if not isinstance(environment, Env): + raise TypeError("Expecting 'environment' to be an instance of `Env`, but got instead: " + "{}".format(type(environment))) self.env = environment # check the policies diff --git a/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_imitation_task.py b/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_imitation_task.py index 2cdedab..cfe374c 100644 --- a/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_imitation_task.py +++ b/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_imitation_task.py @@ -23,7 +23,7 @@ from pyrobolearn.tools.bridges.mouse_keyboard.bridge_mousekeyboard_world import __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "GNU GPLv3" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" @@ -86,6 +86,7 @@ class BridgeMouseKeyboardImitationTask(BridgeMouseKeyboardWorld): # Bridge): Initialize the Bridge between a Mouse-Keyboard interface and an imitation learning task. Args: + world (World): world instance. interface (MouseKeyboardInterface, Env, World): mouse keyboard interface. If the interface is an instance of Env, World, or Simulator, it will create automatically a mouse-keyboard interface. @@ -121,6 +122,7 @@ class BridgeMouseKeyboardImitationTask(BridgeMouseKeyboardWorld): # Bridge): self.visual_points = [] # {} # self.display_trajectories = True + # mapping from keys to methods self.events_fn = {(Key.x,): self.change_camera_view_x, (Key.y,): self.change_camera_view_y, (Key.z,): self.change_camera_view_z, @@ -163,6 +165,11 @@ class BridgeMouseKeyboardImitationTask(BridgeMouseKeyboardWorld): # Bridge): (Key.left_arrow,): lambda: None, (Key.right_arrow,): lambda: None} + # replace key tuples by frozenset + for key in list(self.events_fn.keys()): + value = self.events_fn[key] + self.events_fn[frozenset(key)] = value + # define few variables self.enable_training = False self.enable_recording = False @@ -367,7 +374,7 @@ class BridgeMouseKeyboardImitationTask(BridgeMouseKeyboardWorld): # Bridge): # self.visual_points[key] = bodyId # self.display_trajectories = not self.display_trajectories for i in range(len(self.visual_points[:-1])): - self.simulator.addUserDebugLine(self.visual_points[i], self.visual_points[i + 1], RGBColor.red, 1., 2.) + self.simulator.add_user_debug_line(self.visual_points[i], self.visual_points[i + 1], RGBColor.red, 1., 2.) def reset_visual_points(self): # for key in self.visual_points: diff --git a/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_world.py b/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_world.py index e6f2bbf..31c74a5 100644 --- a/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_world.py +++ b/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_world.py @@ -22,7 +22,7 @@ from pyrobolearn.tools.bridges import Bridge __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "GNU GPLv3" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" @@ -105,6 +105,7 @@ class BridgeMouseKeyboardWorld(Bridge): self.pausing = False self.debug = verbose + # mapping from keys to methods self.events_fn = {(Key.x,): self.change_camera_view_x, (Key.y,): self.change_camera_view_y, (Key.z,): self.change_camera_view_z, @@ -136,6 +137,11 @@ class BridgeMouseKeyboardWorld(Bridge): (Key.left_arrow,): lambda: None, (Key.right_arrow,): lambda: None} + # replace key tuples by frozenset + for key in list(self.events_fn.keys()): + value = self.events_fn[key] + self.events_fn[frozenset(key)] = value + # self.vs = self.simulator.createVisualShape(self.simulator.GEOM_SPHERE, radius=0.02, rgbaColor=(0, 0, 1, 1)) # self.vs1 = self.simulator.createVisualShape(self.simulator.GEOM_SPHERE, radius=0.2, rgbaColor=(1, 0, 0, 1)) # self.vs2 = self.simulator.createVisualShape(self.simulator.GEOM_SPHERE, radius=0.2, rgbaColor=(0, 1, 0, 1)) @@ -285,12 +291,12 @@ class BridgeMouseKeyboardWorld(Bridge): self.print_debug('unselect robot/link') self.robot, self.link_id = None, None - def add_world_text(self, string, position, color=(0.,0.,0.), size=1., lifetime=0.): + def add_world_text(self, string, position, color=(0., 0., 0.), size=1., lifetime=0.): """Add world text.""" self.print_debug('add world text') self.simulator.add_user_debug_text(string, position, color, size, lifetime) - def add_screen_text(self, string, world_position, color=(0.,0.,0.), size=1., lifetime=0.): + def add_screen_text(self, string, world_position, color=(0., 0., 0.), size=1., lifetime=0.): """Add screen text.""" self.print_debug('add screen text') V, P, Vp, V_inv, P_inv, Vp_inv = self.world_camera.get_matrices(True) @@ -300,7 +306,7 @@ class BridgeMouseKeyboardWorld(Bridge): def check_key_events(self): # call function corresponding to key combination if self.interface.key_pressed: - key = tuple(self.interface.key_pressed) + key = frozenset(self.interface.key_pressed) if key in self.events_fn: self.events_fn[key]() @@ -329,11 +335,11 @@ class BridgeMouseKeyboardWorld(Bridge): # if collision, proceed the inverse operation to get the depth on the screen if len(collision) > 0: object_id, link_id, hit_frac, hit_pos, hit_normal = collision[0] - # self.simulator.addUserDebugLine(list(x_world_init[:3]), list(x_world_final[:3]), (0, 0, 1)) - # bodyId = self.simulator.createMultiBody(baseMass=0, baseVisualShapeIndex=self.vs1, - # basePosition=list(x_world_init[:3])) - # bodyId = self.simulator.createMultiBody(baseMass=0, baseVisualShapeIndex=self.vs2, - # basePosition=list(x_world_final[:3])) + # self.simulator.add_user_debug_line(list(x_world_init[:3]), list(x_world_final[:3]), (0, 0, 1)) + # bodyId = self.simulator.create_body(mass=0, visual_shape_id=self.vs1, + # position=list(x_world_init[:3])) + # bodyId = self.simulator.create_body(mass=0, visual_shape_id=self.vs2, + # position=list(x_world_final[:3])) if object_id != -1 and self.world.is_robot_id(object_id): # valid object # Set robot and link_id @@ -381,8 +387,8 @@ class BridgeMouseKeyboardWorld(Bridge): # # draw some spheres on the plane # if self.display_trajectories: - # bodyId = self.simulator.createMultiBody(baseMass=0, baseVisualShapeIndex=self.vs, - # basePosition=point) + # bodyId = self.simulator.create_body(mass=0, visual_shape_id=self.vs, + # position=point) # self.visual_points[tuple(point)] = bodyId # else: # self.visual_points[tuple(point)] = None @@ -391,7 +397,7 @@ class BridgeMouseKeyboardWorld(Bridge): # if self.display_trajectories: # self.visual_points.append(point) # if len(self.visual_points) > 1: - # self.simulator.addUserDebugLine(self.visual_points[-2], self.visual_points[-1], + # self.simulator.add_user_debug_line(self.visual_points[-2], self.visual_points[-1], # RGBColor.red, 1., 2.) # # perform inverse kinematics