diff --git a/examples/kinematics/README.md b/examples/kinematics/README.md index 951c700..c4f317b 100644 --- a/examples/kinematics/README.md +++ b/examples/kinematics/README.md @@ -2,6 +2,19 @@ We provide examples on how to perform forward and inverse kinematics. +Here are the forward kinematics (FK) examples that the user can try: +1. `fk.py`: simple forward kinematics example where we directly sent desired joint positions to the Kuka +manipulator. + +Here are the inverse kinematics (IK) examples that the user can try: +1. `ik.py`: simple inverse kinematics example where the Kuka manipulator has to reach a certain target position +in the world. In this example, the user can also choose the damped-least-squares IK solver. +2. `ik_libraries.py`: comparison between different IK libraries including `pybullet`, `PyKDL`, `trac_ik`, and +`rbdl` using the Kuka manipulator. +3. `moving_sphere.py`: damped-least-squares IK with the Kuka manipulator where the goal is to follow a sphere +that moves in a circular manner. + + References: - [1] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010 - [2] "Springer Handbook of Robotics", Siciliano et al., 2008 diff --git a/examples/kinematics/inverse/ik.py b/examples/kinematics/inverse/ik.py index 6f0f28a..a5bc89b 100644 --- a/examples/kinematics/inverse/ik.py +++ b/examples/kinematics/inverse/ik.py @@ -8,16 +8,25 @@ Set the `solver_flag` to a number between 0 and 1 (see lines [19,22]) to select import numpy as np from itertools import count +import argparse from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import KukaIIWA +# create parser to select the IK solver +parser = argparse.ArgumentParser() +parser.add_argument('-s', '--solver', help='the IK solver to select (0: use robot.calculate_inverse_kinematics(), ' + '1: use damped-least-squares IK using Jacobian)', type=int, + choices=[0, 1], default=1) +args = parser.parse_args() + + # select IK solver, by setting the flag: # 0 = pybullet + calculate_inverse_kinematics() # 1 = pybullet + damped-least-squares IK using Jacobian (provided by pybullet) -solver_flag = 1 # 1 and 4 gives pretty good results +solver_flag = args.solver # 1 gives a pretty good result # Create simulator @@ -34,6 +43,7 @@ robot.print_info() dt = 1./240 link_id = robot.get_end_effector_ids(end_effector=0) joint_ids = robot.joints # actuated joint +# joint_ids = joint_ids[2:] damping = 0.01 # for damped-least-squares IK wrt_link_id = -1 # robot.get_link_ids('iiwa_link_1') @@ -41,11 +51,10 @@ wrt_link_id = -1 # robot.get_link_ids('iiwa_link_1') xd = np.array([0.5, 0., 0.5]) world.load_visual_sphere(xd, radius=0.05, color=(1, 0, 0, 0.5)) -# joint_ids = joint_ids[2:] - +# change the robot visual robot.change_transparency() -robot.draw_link_frames([-1, 0]) -robot.draw_bounding_boxes(joint_ids[0]) +robot.draw_link_frames(link_ids=[-1, 0]) +robot.draw_bounding_boxes(link_ids=joint_ids[0]) # robot.draw_link_coms([-1,0]) qIdx = robot.get_q_indices(joint_ids) diff --git a/examples/kinematics/inverse/ik_libraries.py b/examples/kinematics/inverse/ik_libraries.py index a736247..ab8d51f 100644 --- a/examples/kinematics/inverse/ik_libraries.py +++ b/examples/kinematics/inverse/ik_libraries.py @@ -14,40 +14,23 @@ Set the `solver_flag` to a number between 0 and 4 (see lines [53,60]) to select import os import numpy as np from itertools import count +import argparse from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import KukaIIWA -# import PyKDL -try: - import PyKDL as kdl -except ImportError as e: - raise ImportError(repr(e) + '\nTry to install `PyKDL`: ' - 'sudo apt-get install ros--python-orocos-kdl' - 'or install it manually from `https://github.com/orocos/orocos_kinematics_dynamics`') - -# import kdl_parser_py -try: - import kdl_parser_py.urdf as KDLParser -except ImportError as e: - raise ImportError(repr(e) + '\nTry to install `kdl_parser_py`: ' - 'sudo apt-get install ros--kdl-parser-py') - -# import track_ik_python -try: - from trac_ik_python.trac_ik import IK as TracIK -except ImportError as e: - raise ImportError(repr(e) + '\nTry to install `trac_ik_python`: ' - 'sudo apt-get install ros--trac-ik-python') - -# import rbdl -try: - import rbdl -except ImportError as e: - raise ImportError(repr(e) + '\nTry to install `rbdl` manually from `https://bitbucket.org/rbdl/rbdl`') - +# create parser to select the IK solver +parser = argparse.ArgumentParser() +parser.add_argument('-s', '--solver', help='the IK solver to select:\n' + '0: use robot.calculate_inverse_kinematics()\n' + '1: use damped-least-squares IK using Jacobian (provided by simulator)\n' + '2: use PyKDL\n' + '3: use trac_ik\n' + '4: use rbdl + damped-least-squares IK using Jacobian (provided by rbdl)', + type=int, choices=[0, 1, 2, 3, 4], default=1) +args = parser.parse_args() # TO BE SET BY THE USER # select IK solver, by setting the flag: @@ -56,7 +39,39 @@ except ImportError as e: # 2 = PyKDL # 3 = trac_ik # 4 = rbdl + damped-least-squares IK using Jacobian (provided by rbdl) -solver_flag = 1 # 1 and 4 gives pretty good results +solver_flag = args.solver # 1 and 4 gives pretty good results + +if solver_flag == 2: # PyKDL + # import PyKDL + try: + import PyKDL as kdl + except ImportError as e: + raise ImportError(repr(e) + '\nTry to install `PyKDL`: ' + 'sudo apt-get install ros--python-orocos-kdl' + 'or install it manually from ' + '`https://github.com/orocos/orocos_kinematics_dynamics`') + + # import kdl_parser_py + try: + import kdl_parser_py.urdf as kdl_parser + except ImportError as e: + raise ImportError(repr(e) + '\nTry to install `kdl_parser_py`: ' + 'sudo apt-get install ros--kdl-parser-py') + +elif solver_flag == 3: # trac_ik_python + # import trac_ik_python + try: + from trac_ik_python.trac_ik import IK as trac_ik + except ImportError as e: + raise ImportError(repr(e) + '\nTry to install `trac_ik_python`: ' + 'sudo apt-get install ros--trac-ik-python') + +elif solver_flag == 4: # rbdl + # import rbdl + try: + import rbdl + except ImportError as e: + raise ImportError(repr(e) + '\nTry to install `rbdl` manually from `https://bitbucket.org/rbdl/rbdl`') # Create simulator @@ -181,7 +196,7 @@ elif solver_flag == 1: ################## elif solver_flag == 2: print("Using PyKDL:") - model = KDLParser.treeFromFile(urdf) + model = kdl_parser.treeFromFile(urdf) if model[0]: model = model[1] else: @@ -237,7 +252,7 @@ elif solver_flag == 3: urdf_string = open(urdf, 'r').read() # create IK solver - ik_solver = TracIK(base_link=base_name, tip_link=end_effector_name, urdf_string=urdf_string, solve_type='Distance') + ik_solver = trac_ik(base_link=base_name, tip_link=end_effector_name, urdf_string=urdf_string, solve_type='Distance') # define upper and lower limits (optional) # lb, ub = -np.ones(6)*100, np.ones(6)*100 diff --git a/examples/worlds/README.md b/examples/worlds/README.md index 47368eb..5831848 100644 --- a/examples/worlds/README.md +++ b/examples/worlds/README.md @@ -9,6 +9,8 @@ The world is usually created once the simulator has been selected. Here are the examples that the user can try: 1. `load_world.py`: load a basic world (i.e. with a floor and gravity enabled) with different objects (only visual, and with collisions) that are movable, fixed, or are moving. -2. `load_robot.py`: load a robot in a basic world and distribute randomly few objects on the floor. -3. `load_heightmap.py`: load a terrain from a heightmap (png) and load a robot on it. -4. `generate_terrain.py`: generate a terrain and distribute randomly few objects on the terrain. +2. `follow_moving_body.py`: follow a moving body with the world camera. +3. `move_camera.py`: get the world camera and move it in the world using the keyboard interface. +4. `load_robot.py`: load a robot in a basic world and distribute randomly few objects on the floor. +5. `load_heightmap.py`: load a terrain from a heightmap (png) and load a robot on it. +6. `generate_terrain.py`: generate a terrain and distribute randomly few objects on the terrain. diff --git a/examples/worlds/follow_moving_body.py b/examples/worlds/follow_moving_body.py new file mode 100644 index 0000000..1a98020 --- /dev/null +++ b/examples/worlds/follow_moving_body.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python +"""Follow a body with the main camera in the world. + +Try to move the sphere with the mouse (left-click on the object). +""" + +from itertools import count + +import pyrobolearn as prl + +# create simulator +sim = prl.simulators.Bullet() + +# create basic world (with a floor and gravity enabled by default) +world = prl.worlds.BasicWorld(sim) + +# load sphere +sphere = world.load_sphere(position=[0, 0, 15.], radius=0.2, mass=1, color=(1, 0, 0, 1)) + +# run simulator +for _ in count(): + # follow sphere + world.follow(sphere) + # perform one step in the world + world.step(sleep_dt=1. / 240) diff --git a/examples/worlds/move_camera.py b/examples/worlds/move_camera.py new file mode 100644 index 0000000..010247a --- /dev/null +++ b/examples/worlds/move_camera.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python +"""Move main camera in the world. + +Move the main camera in the world using the keyboard: +- top arrow: move forward +- bottom arrow: move backward +- left arrow: move sideways to the left +- right arrow: move sideways to the right +- ctrl + top arrow: turn downward +- ctrl + bottom arrow: turn upward +- ctrl + left arrow: turn to the right +- ctrl + right arrow: turn to the left +""" + +from itertools import count + +import pyrobolearn as prl + +# create simulator +sim = prl.simulators.Bullet() + +# create basic world (with a floor and gravity enabled by default) +world = prl.worlds.BasicWorld(sim) + +# get world camera +camera = world.camera + +# create mouse-keyboard interface +interface = prl.tools.interfaces.MouseKeyboardInterface(sim) + +# run simulator +for _ in count(): + # perform a step with the interface (i.e. get events from interface) + interface.step() + + # check the keys that are down + key, key_down = interface.key, interface.key_down + if key.ctrl in key_down: + if key.top_arrow in key_down: + camera.pitch -= 0.005 # turn downward + elif key.bottom_arrow in key_down: + camera.pitch += 0.005 # turn upward + elif key.left_arrow in key_down: + camera.yaw -= 0.005 # turn to the right + elif key.right_arrow in key_down: + camera.yaw += 0.005 # turn to the left + else: + if key.top_arrow in key_down: + camera.target_position += 0.01 * camera.forward_vector # move forward + elif key.bottom_arrow in key_down: + camera.target_position -= 0.01 * camera.forward_vector # move backward + elif key.left_arrow in key_down: + camera.target_position -= 0.01 * camera.lateral_vector # move to the left + elif key.right_arrow in key_down: + camera.target_position += 0.01 * camera.lateral_vector # move to the right + + # perform one step in the world + world.step(sleep_dt=1. / 254) diff --git a/pyrobolearn/tools/interfaces/mouse_keyboard/mousekeyboard.py b/pyrobolearn/tools/interfaces/mouse_keyboard/mousekeyboard.py index 13a0696..05c2cb5 100644 --- a/pyrobolearn/tools/interfaces/mouse_keyboard/mousekeyboard.py +++ b/pyrobolearn/tools/interfaces/mouse_keyboard/mousekeyboard.py @@ -97,7 +97,9 @@ class MouseKeyboardInterface(InputInterface): self.mouse_x, self.mouse_y = 0, 0 # define variables for key events - self.key_pressed = [] + self.key_pressed = set([]) + self.key_down = set([]) + self.key = Key ############## # Properties # @@ -151,18 +153,24 @@ class MouseKeyboardInterface(InputInterface): events = self.simulator.get_keyboard_events() # create new list of key pressed - self.key_pressed = [] + self.key_pressed, self.key_down = set([]), set([]) if Key.shift in events: - self.key_pressed.append(Key.shift) + self.key_pressed.add(Key.shift) + self.key_down.add(Key.shift) if Key.alt in events: - self.key_pressed.append(Key.alt) + self.key_pressed.add(Key.alt) + self.key_down.add(Key.alt) if Key.ctrl in events: - self.key_pressed.append(Key.ctrl) + self.key_pressed.add(Key.ctrl) + self.key_down.add(Key.ctrl) # go through each keyboard event for key, state in events.items(): - if state == Key.pressed: # or state == Key.down: # the key is pressed or down - self.key_pressed.append(key) + if state == Key.pressed: # if the key is pressed + self.key_pressed.add(key) + self.key_down.add(key) # a key pressed is also a key down + elif state == Key.down: # if the key is down + self.key_down.add(key) def check_mouse_events(self): """Check the mouse events.""" diff --git a/pyrobolearn/worlds/world.py b/pyrobolearn/worlds/world.py index 686c3b1..040166c 100644 --- a/pyrobolearn/worlds/world.py +++ b/pyrobolearn/worlds/world.py @@ -394,15 +394,15 @@ class World(object): Follow the given body with the world camera at the specified distance, yaw and pitch angles. Args: - body (Body): body to follow with the world camera. + body (Body, int, long): body (or body id) to follow with the world camera. distance (float, None): distance (in meter) from the camera and the body position. If None, it will take the current distance. yaw (float, None): camera yaw angle (in radians) left/right. If None, it will take the current yaw angle. pitch (float, None): camera pitch angle (in radians) up/down. If None, it will take the current pitch angle. """ - if not isinstance(body, Body): - raise TypeError("Expecting the given body to be an instance of `Body`, instead got: {}".format(type(body))) - self.camera.reset(distance=distance, yaw=yaw, pitch=pitch, target_position=body.position) + if isinstance(body, Body): + body = body.id + self.camera.follow(body_id=body, distance=distance, yaw=yaw, pitch=pitch) def load_robot(self, robot, position=None, orientation=None, fixed_base=None, *args, **kwargs): """ diff --git a/pyrobolearn/worlds/world_camera.py b/pyrobolearn/worlds/world_camera.py index 39cdb55..6b30c51 100644 --- a/pyrobolearn/worlds/world_camera.py +++ b/pyrobolearn/worlds/world_camera.py @@ -7,11 +7,15 @@ Dependencies: - `pyrobolearn.simulators` """ +import sys import numpy as np from pyrobolearn.utils.transformation import get_quaternion_from_matrix, get_rpy_from_matrix, get_rpy_from_quaternion from pyrobolearn.simulators import Simulator +# define long for Python 3.x +if int(sys.version[0]) == 3: + long = int __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" @@ -167,6 +171,14 @@ class WorldCamera(object): """ return self.sim.get_debug_visualizer()[5] + @property + def lateral_vector(self): + """ + Return the lateral axis of the camera (=cross product between forward and up vectors) + """ + up_vector, forward_vector = self.sim.get_debug_visualizer()[4:6] + return np.cross(forward_vector, up_vector) + @property def yaw(self): """ @@ -287,7 +299,6 @@ class WorldCamera(object): # Methods # ########### - # alias def get_debug_visualizer_camera(self): """ Return all the information provided by the camera. @@ -577,6 +588,54 @@ class WorldCamera(object): """Return the representation string of the object.""" return self.__class__.__name__ + def print_info(self): + """Print information about the camera. + + int: width of the visualizer camera (in pixel) + int: height of the visualizer camera (in pixel) + np.float[4,4]: view matrix [4,4] + np.float[4,4]: perspective projection matrix [4,4] + np.float[3]: camera up vector expressed in the Cartesian world space + np.float[3]: forward axis of the camera expressed in the Cartesian world space + np.float[3]: This is a horizontal vector that can be used to generate rays (for mouse picking or creating + a simple ray tracer for example) + np.float[3]: This is a vertical vector that can be used to generate rays (for mouse picking or creating a + simple ray tracer for example) + float: yaw angle (in radians) of the camera, in Cartesian local space coordinates + float: pitch angle (in radians) of the camera, in Cartesian local space coordinates + float: distance between the camera and the camera target + np.float[3]: target of the camera, in Cartesian world space coordinates + """ + info = self.info + view_inv = np.linalg.inv(info[2]) + position = view_inv[:3, 3] + orientation = get_quaternion_from_matrix(view_inv[:3, :3]) + print("\nCamera width and height: {}, {}".format(info[0], info[1])) + print("Camera position: {}".format(position)) + print("Camera orientation (quaternion [x,y,z,w]): {}".format(orientation)) + print("Camera target position: {}".format(info[11])) + print("Camera yaw and pitch angles (deg): {}, {}".format(np.rad2deg(info[8]), np.rad2deg(info[9]))) + print("Camera distance: {}".format(info[10])) + print("Camera forward vector: {}".format(info[5])) + print("Camera up vector: {}".format(info[4])) + + def follow(self, body_id, distance=None, yaw=None, pitch=None): + """ + Follow the given body in the simulator with the world camera at the specified distance, yaw and pitch angles. + + Args: + body_id (int, long): body to follow with the world camera. + distance (float, None): distance (in meter) from the camera and the body position. If None, it will take + the current distance. + yaw (float, None): camera yaw angle (in radians) left/right. If None, it will take the current yaw angle. + pitch (float, None): camera pitch angle (in radians) up/down. If None, it will take the current pitch angle. + """ + if not isinstance(body_id, (int, long)): + raise TypeError("Expecting the given 'body_id' to be a unique id (int/long) returned by the simulator, " + "instead got: {}".format(type(body_id))) + target_position = self.sim.get_base_position(body_id) + self.reset(distance=distance, yaw=yaw, pitch=pitch, target_position=target_position) + # Tests if __name__ == '__main__': @@ -619,4 +678,4 @@ if __name__ == '__main__': # step in the simulator sim.step() - time.sleep(1./254) + time.sleep(1./240)