add 2 examples with world camera + corresponding updates

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

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)

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-<distribution>-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-<distribution>-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-<distribution>-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-<distribution>-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-<distribution>-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-<distribution>-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

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.

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)

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)

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."""

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):
         """

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)