diff --git a/examples/robots/README.md b/examples/robots/README.md deleted file mode 100644 index e945c60..0000000 --- a/examples/robots/README.md +++ /dev/null @@ -1,22 +0,0 @@ -## Robot examples - -More than 60 robots (of various types) are available through `pyrobolearn`. - -Here are the few examples that you can find in this folder: -1. `load_robot.py `: load the given robot in the world. -2. `visualize_robot.py `: test different visualization tools that can be used on the robot to show its -joint axis, bounding boxes, and others. -3. `robot_with_sliders.py `: load the given robot in the world and allow you to manipulate the robot's -joints with sliders. -4. `distribute_epucks.py`: distribute several e-pucks in the world and make them move forward. -5. `quadcopter_controller.py`: move a quadcopter in the air using an Xbox or Playstation game controller. -6. `robots/.py`: load the given robot in the simulator by directly instantiating it. Some of these files do -more than just loading the robot. - -Notes: to turn the camera in the simulator, keep pressing the `ctrl` key and the left button on the mouse, and -move this last one. - - -#### What to check next? - -Check the `pyrobolearn/examples/interfaces` or `pyrobolearn/examples/kinematics` folder. diff --git a/examples/robots/README.rst b/examples/robots/README.rst new file mode 100644 index 0000000..3a5a173 --- /dev/null +++ b/examples/robots/README.rst @@ -0,0 +1,28 @@ +Robot examples +============== + +More than 60 robots (of various types) are available through ``pyrobolearn``. + +Here are the few examples that you can find in this folder: + +1. ``load_robot.py ``: load the given robot in the world. +2. ``visualize_robot.py ``: test different visualization tools that can be used on the robot to show its + joint axis, bounding boxes, and others. +3. ``robot_with_sliders.py ``: load the given robot in the world and allow you to manipulate the robot's + joints with sliders. +4. ``distribute_epucks.py``: distribute several e-pucks in the world and make them move forward. +5. ``quadcopter_controller.py``: move a quadcopter in the air using an Xbox or Playstation game controller. +6. ``robots/.py``: load the given robot in the simulator by directly instantiating it. Some of these files do + more than just loading the robot. +7. ``attach_gripper_to_manipulator``: attach the specified gripper / hand to the Kuka manipulator robot. In this file, + you can check the various grippers you can use. +8. ``attach_manipulator_to_quadruped``: attach the Kuka Youbot manipulator to the HyQ2Max quadruped robot. + +Notes: to turn the camera in the simulator, keep pressing the ``ctrl`` key and the left button on the mouse, and +move the mouse. + + +What to check next? +~~~~~~~~~~~~~~~~~~~ + +Check the ``pyrobolearn/examples/interfaces`` or ``pyrobolearn/examples/kinematics`` folder. diff --git a/examples/robots/attach_gripper_to_manipulator.py b/examples/robots/attach_gripper_to_manipulator.py new file mode 100644 index 0000000..5c9ff28 --- /dev/null +++ b/examples/robots/attach_gripper_to_manipulator.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python +"""Attach a gripper/hand to the Kuka manipulator. + +In this file, you can attach different grippers / hands to the kuka robot. You can move the robot with the mouse. +""" + +import argparse + +import pyrobolearn as prl + + +# create parser to select the gripper/hand +parser = argparse.ArgumentParser() +parser.add_argument('-g', '--gripper', help='the gripper/hand to attach to the kuka robot', type=str, + choices=['softhand', 'allegrohand', 'wam_gripper', 'youbot_gripper', 'pr2_gripper', 'jaco_gripper', + 'fetch_gripper', 'franka_gripper', 'baxter_gripper', 'schunk_hand', 'shadowhand'], + default='softhand') +args = parser.parse_args() + + +# create simulator +sim = prl.simulators.Bullet() + +# create basic world with floor and gravity +world = prl.worlds.BasicWorld(sim) + +# load kuka robot +robot = world.load_robot('kuka_iiwa') + +# load hand/gripper +hand = world.load_robot(args.gripper, position=(0., 0., 1.5), fixed_base=False) + +# compute parent frame position (this will be removed later and integrated in PRL) +parent_frame_position = [0., 0., 0.] +if args.gripper == 'shadowhand': + parent_frame_position = [0., 0., 0.1] +elif args.gripper == 'allegrohand': + parent_frame_position = [0., 0., 0.06] +elif args.gripper == 'wam_gripper': + parent_frame_position = [0., 0., 0.01] +elif args.gripper == 'youbot_gripper': + parent_frame_position = [0., 0., 0.03] +elif args.gripper == 'pr2_gripper': + parent_frame_position = [0., 0., 0.002] +elif args.gripper == 'jaco_gripper': + parent_frame_position = [0., 0., 0.06] +elif args.gripper == 'fetch_gripper': + parent_frame_position = [0., 0., 0.07] +elif args.gripper == 'franka_gripper': + parent_frame_position = [0., 0., 0.02] +elif args.gripper == 'baxter_gripper': + parent_frame_position = [0., 0., -0.03] +elif args.gripper == 'schunk_hand': + parent_frame_position = [0., 0., 0.002] + +# attach hand/gripper to robot +world.attach(body1=robot, body2=hand, link1=robot.end_effectors[0], link2=-1, joint_axis=[0., 0., 0.], + parent_frame_position=parent_frame_position, child_frame_position=[0., 0., 0.]) + +# set the hand joint positions +hand.set_joint_positions([0.] * hand.num_actuated_joints) + +# run simulation +for t in prl.count(): + sim.step(sim.dt) diff --git a/examples/robots/attach_manipulator_to_quadruped.py b/examples/robots/attach_manipulator_to_quadruped.py new file mode 100644 index 0000000..9ac1bed --- /dev/null +++ b/examples/robots/attach_manipulator_to_quadruped.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python +"""Attach the Kuka Youbot manipulator to the HyQ2Max quadruped. +""" + +import pyrobolearn as prl + + +# create simulator +sim = prl.simulators.Bullet() + +# create world +world = prl.worlds.BasicWorld(sim) + +# create quadruped and manipulator +quadruped = world.load_robot('hyq2max') +manipulator = world.load_robot('kuka_youbot_arm', position=(0., 0., 1.), fixed_base=False) + +# attach manipulator to the back of the robot +world.attach(body1=quadruped, body2=manipulator, link1=-1, link2=-1, joint_axis=[0., 0., 0.], + parent_frame_position=[0.25, 0., 0.2], child_frame_position=[0., 0., 0.]) + +# run simulation +for t in prl.count(): + sim.step(sim.dt) diff --git a/pyrobolearn/robots/__init__.py b/pyrobolearn/robots/__init__.py index cc12157..f5e2610 100644 --- a/pyrobolearn/robots/__init__.py +++ b/pyrobolearn/robots/__init__.py @@ -14,13 +14,14 @@ from . import sensors from .robot import Robot # Categories/types of robots -from .legged_robot import * -from .manipulator import * -from .wheeled_robot import * -from .uav import * -from .usv import * -from .uuv import * -from .hand import * +from .legged_robot import LeggedRobot, BipedRobot, QuadrupedRobot, HexapodRobot +from .manipulator import Manipulator, BiManipulator +from .wheeled_robot import WheeledRobot, DifferentialWheeledRobot, AckermannWheeledRobot +from .uav import UAVRobot, FixedWingUAV, RotaryWingUAV, FlappingWingUAV +from .usv import USVRobot +from .uuv import UUVRobot +from .hand import Hand, TwoHand +from .gripper import Gripper, ParallelGripper, AngularGripper, VacuumGripper # Mujoco models from .ant import Ant @@ -138,6 +139,7 @@ implemented_robots.remove('icub') # create dictionary that maps robot names to robot classes robot_names_to_classes = {} +implemented_grippers = [] for robot_name in implemented_robots: module = importlib.import_module('pyrobolearn.robots.' + robot_name) # 'robots.'+robot) # robot_class = getattr(module, robot.capitalize()) @@ -146,6 +148,7 @@ for robot_name in implemented_robots: if inspect.isclass(cls) and issubclass(cls, Robot): if name.lower() == ''.join(robot_name.split('_')): robot_names_to_classes[robot_name] = cls + name = robot_name else: name_list = re.findall('[0-9]*[A-Z]+[0-9]*[a-z]*', name) name = '_'.join([n.lower() for n in name_list]) @@ -158,4 +161,9 @@ for robot_name in implemented_robots: name = 'usv_robot' robot_names_to_classes[name] = cls + # add grippers and hands + if issubclass(cls, Gripper) or issubclass(cls, Hand): + implemented_grippers.append(name) + implemented_robots = set(list(robot_names_to_classes.keys())) +implemented_grippers = set(implemented_grippers) diff --git a/pyrobolearn/simulators/bullet.py b/pyrobolearn/simulators/bullet.py index 8b45f09..1ed346d 100644 --- a/pyrobolearn/simulators/bullet.py +++ b/pyrobolearn/simulators/bullet.py @@ -1579,8 +1579,9 @@ class Bullet(Simulator): VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the applied joint motor torque is exactly what you provide, so there is no need to report it separately. """ - states = self.sim.getJointStates(body_id, joint_ids) + states = list(self.sim.getJointStates(body_id, joint_ids)) for idx, state in enumerate(states): + states[idx] = list(state) states[idx][2] = np.asarray(state[2]) return states diff --git a/pyrobolearn/worlds/world.py b/pyrobolearn/worlds/world.py index 6a7731d..9d7bf6b 100644 --- a/pyrobolearn/worlds/world.py +++ b/pyrobolearn/worlds/world.py @@ -683,6 +683,8 @@ class World(object): position (float[3]): new position of the object. If None, it will keep the old position. orientation (float[4]): new orientation of the object. If None, it will keep the old orientation. """ + if isinstance(body_id, Body): + body_id = body_id.id if position is None: position = self.sim.get_base_pose(body_id)[0] if orientation is None: @@ -706,6 +708,8 @@ class World(object): frame (int): allows to specify the coordinate system of force/position. sim.LINK_FRAME (=1) for local link frame, and sim.WORLD_FRAME (=2) for world frame. By default, it is the world frame. """ + if isinstance(body_id, Body): + body_id = body_id.id self.sim.apply_external_force(body_id, link_id, force, position, frame) def get_body_color(self, body_id): @@ -1010,14 +1014,16 @@ class World(object): def attach(self, body1, body2, link1=-1, link2=-1, joint_axis=(0., 0., 0.), parent_frame_position=(0., 0., 0.), child_frame_position=(0., 0., 0.), - parent_frame_orientation=(0., 0., 0., 1.), child_frame_orientation=(0., 0., 0., 1.)): + parent_frame_orientation=None, child_frame_orientation=(0., 0., 0., 1.)): """ Attach two bodies (links) together at the specified contact point. To detach them, call ``world.detach(body1, body2, link1, link2)``. - Note that this method is syntactic sugar for the ``create_constraint`` method with the joint type set to a - fixed joint. + Note that this method, in addition to call ``create_constraint`` (with a fixed joint), it checks for collisions + between body1 and body2, and if there are, check how much body2 penetrates body1 and recomputes the parent + frame position such that there are no more collisions. Additionally, attaching an object can change the state + of body1, as such we reset the state as it was before calling this method. Args: body1 (int, Body): body unique id, or a Body instance. @@ -1036,13 +1042,75 @@ class World(object): Returns: bool: True if it was successful. """ + # check arguments + if isinstance(body1, Body): + body1 = body1.id + if isinstance(body2, Body): + body2 = body2.id + + if parent_frame_orientation is None: + parent_frame_orientation = self.sim.get_base_orientation(body2) + + # save the state of body1 + pose = self.sim.get_base_pose(body1) + velocity = self.sim.get_base_velocity(body1) + joint_ids = [joint_id for joint_id in range(self.sim.num_joints(body1)) + if self.sim.get_joint_info(body1, joint_id)[2] != self.sim.JOINT_FIXED] + joint_states = self.sim.get_joint_states(body1, joint_ids=joint_ids) + + # create constraint constraint_id = self.create_constraint(parent_body=body1, parent_link_id=link1, child_body=body2, child_link_id=link2, joint_axis=joint_axis, parent_frame_position=parent_frame_position, child_frame_position=child_frame_position, parent_frame_orientation=parent_frame_orientation, child_frame_orientation=child_frame_orientation) + + # advance for few steps (in simulation) + for i in range(10): + self.step() + + # check collisions + collisions = self.sim.get_contact_points(body1, body2, link1_id=link1, link2_id=link2) + + # if collisions, remove the constraint and recompute the parent frame position such that there are no more + # collisions. This is achieved by checking the penetrating distance and moving along the contact normal + # direction (pointing from body1 to body2) by minus that amount. + if len(collisions) > 0: + # TODO: it seems that with PyBullet this does nothing, they check for collisions when creating the + # constraint + # get worst penetrating distance and associated contact normal direction (from body2 to body1) + worst_distance, worst_normal_direction = 0, None + for collision in collisions: + distance, normal_direction = collision[7:9] + if distance < worst_distance: + worst_distance, worst_normal_direction = distance, normal_direction + + # compute new parent frame position (the 0.002 is a safety margin) + parent_frame_position = np.asarray(parent_frame_position) + parent_frame_position += (-worst_distance + 0.002) * -worst_normal_direction + + # remove the previous constraint + self.remove_constraint(constraint_id) + + # recreate the constraint with the correct parent frame position + constraint_id = self.create_constraint(parent_body=body1, parent_link_id=link1, child_body=body2, + child_link_id=link2, joint_axis=joint_axis, + parent_frame_position=parent_frame_position, + child_frame_position=child_frame_position, + parent_frame_orientation=parent_frame_orientation, + child_frame_orientation=child_frame_orientation) + + # remember the constraint such that we can call later ``detach`` without providing too much information. self.constraints[(body1, body2)] = {(link1, link2): constraint_id} + + # restore state + self.sim.reset_base_pose(body1, position=pose[0], orientation=pose[1]) + self.sim.reset_base_velocity(body1, linear_velocity=velocity[0], angular_velocity=velocity[1]) + for joint_id, joint_state in zip(joint_ids, joint_states): + self.sim.reset_joint_state(body1, joint_id, position=joint_state[0], velocity=joint_state[1]) + + # return constraint id return constraint_id > 0 def detach(self, body1, body2, link1=None, link2=None): @@ -1062,8 +1130,7 @@ class World(object): """ if (body1, body2) in self.constraints: if link1 is None or link2 is None: - for constraint in self.constraints[(body1, body2)]: - link_id1, link_id2, constraint_id = constraint[-1] + for (link_id1, link_id2), constraint_id in self.constraints[(body1, body2)].items(): if link1 is None: if link2 is None: # link1 and link2 are both None self.sim.remove_constraint(constraint_id) @@ -1101,8 +1168,7 @@ class World(object): if link1 is None and link2 is None: return True else: - for constraint in self.constraints[(body1, body2)]: - link_1, link_2, constraint_id = constraint[-1] + for link_1, link_2 in self.constraints[(body1, body2)].keys(): if link1 == link_1 and link2 == link_2: return True return False @@ -2081,6 +2147,8 @@ class World(object): body_id (int): unique body id. link_id (int): link id. If -1, it will be the base. """ + if isinstance(body_id, Body): + body_id = body_id.id texture = self.sim.load_texture(texture) self.sim.change_visual_shape(object_id=body_id, link_id=link_id, texture_id=texture)