add dynamics examples

examples/dynamics/README.md

@@ -0,0 +1,9 @@
+## Robot dynamics
+
+We provide examples on how to perform forward and inverse dynamics, force and impedance control.
+
+References:
+- [1] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010
+- [2] "Springer Handbook of Robotics", Siciliano et al., 2008
+- [3] "Rigid Body Dynamics Algorithms", Featherstone, 2008
+- [4] [Lecture on Impedance Control](http://www.diag.uniroma1.it/~deluca/rob2_en/15_ImpedanceControl.pdf) by Prof. De Luca, Universita di Roma,

examples/dynamics/control/README.md

@@ -0,0 +1,8 @@
+### Force and Impedance control
+
+In a nutshell, you have different control modes:
+
+* position control: specify the desired task (or joint) positions
+* velocity control: specify the desired task (or joint) velocities
+* force control: specify the desired task (or joint) forces
+* impedance control: specify the task (or joint) forces based on the desired task (or joint) positions, velocities, and accelerations.

examples/dynamics/control/force/gravity_compensation.py

@@ -0,0 +1,41 @@
+#!/usr/bin/env python
+"""Force control: gravity compensation with RRBot
+
+Try to move the end-effector using the mouse, and see what happens. Compare the obtained results with
+`force/no_forces.py` and `impedance/attractor_point.py`.
+"""
+
+import numpy as np
+from itertools import count
+
+import pyrobolearn as prl
+
+
+# Create simulator
+sim = prl.simulators.Bullet()
+
+# create world
+world = prl.worlds.BasicWorld(sim)
+
+# load robot
+robot = prl.robots.RRBot(sim)
+robot.disable_motor()       # disable motors; comment the `robot.set_joint_torques(torques)` to see what happens
+robot.print_info()
+robot.change_transparency()
+
+
+# run simulator
+for _ in count():
+    # get current joint positions, velocities, accelerations
+    q = robot.get_joint_positions()
+    dq = robot.get_joint_velocities()
+    ddq = np.zeros(len(q))
+
+    # compute torques (Coriolis, centrifugal and gravity compensation) using inverse dynamics
+    torques = robot.calculate_inverse_dynamics(ddq, dq, q)
+
+    # force control
+    robot.set_joint_torques(torques=torques)
+
+    # perform a step in the world
+    world.step(sleep_dt=sim.dt)

examples/dynamics/control/force/no_forces.py

@@ -0,0 +1,34 @@
+#!/usr/bin/env python
+"""Force control: apply no forces/torques.
+
+Try to move the end-effector using the mouse, and see what happens. Compare the obtained results with
+`force/gravity_compensation.py` and `impedance/attractor_point.py`.
+"""
+
+import numpy as np
+from itertools import count
+
+import pyrobolearn as prl
+
+
+# Create simulator
+sim = prl.simulators.Bullet()
+
+# create world
+world = prl.worlds.BasicWorld(sim)
+
+# load robot
+robot = prl.robots.RRBot(sim)
+robot.disable_motor()       # disable motors
+robot.print_info()
+robot.change_transparency()
+
+
+# run simulator
+for _ in count():
+
+    # force control: apply no forces/torques
+    robot.set_joint_torques(torques=np.zeros(len(robot.joints)))
+
+    # perform a step in the world
+    world.step(sleep_dt=sim.dt)

examples/dynamics/control/impedance/attractor_point.py

@@ -0,0 +1,69 @@
+#!/usr/bin/env python
+"""Attractor point using impedance control with RRBot
+
+Try to move the end-effector using the mouse, and see what happens. Compare the obtained results with
+`force/no_forces.py` and `force/gravity_compensation.py`.
+"""
+
+import numpy as np
+from itertools import count
+
+import pyrobolearn as prl
+
+
+# Create simulator
+sim = prl.simulators.Bullet()
+
+# create world
+world = prl.worlds.BasicWorld(sim)
+
+# load robot
+robot = prl.robots.RRBot(sim)
+robot.disable_motor()           # disable motors; comment the `robot.set_joint_torques(torques)` to see what happens
+robot.print_info()
+robot.change_transparency()
+
+# define variables
+link_id = robot.get_link_ids('hokuyo_link')         # the link we are interested to
+com_frame = robot.get_link_states(link_id)[2]
+x_des = robot.get_link_world_positions(link_id)     # desired cartesian position
+
+# gains
+K = 100 * np.identity(3)
+D = 2 * np.sqrt(K)  # critically damped
+D = 3 * D           # manually increase damping
+
+# draw a sphere at the desired location
+world.load_visual_sphere(position=x_des, radius=0.1, color=(0, 1, 0, 0.5))
+
+
+# run simulator
+for _ in count():
+    # get current joint positions, velocities, accelerations
+    q = robot.get_joint_positions()
+    dq = robot.get_joint_velocities()
+    ddq = np.zeros(len(q))
+
+    # get current link position and velocity
+    x = robot.get_link_world_positions(link_id)
+    dx = robot.get_link_world_linear_velocities(link_id)
+
+    # compute torques (Coriolis, centrifugal and gravity compensation) using inverse dynamics
+    torques = robot.calculate_inverse_dynamics(ddq, dq, q)
+
+    # get linear jacobian
+    Jlin = robot.get_linear_jacobian(link_id=link_id, local_position=com_frame)
+
+    # attractor point: compute cartesian forces (PD control)
+    F = K.dot(x_des - x) - D.dot(dx)
+
+    # add torques resulting from them
+    # torques += Jlin.T.dot(F)
+    # torques += Jlin.T.dot(- D.dot(dx))  # active compliance
+    # torques = Jlin.T.dot(F)             #
+
+    # impedance control
+    robot.set_joint_torques(torques=torques)
+
+    # perform a step in the world
+    world.step(sleep_dt=1./240)

pyrobolearn/robots/coman.py

@@ -69,11 +69,11 @@ class Coman(BipedRobot, BiManipulatorRobot):
 # Test
 if __name__ == "__main__":
     from itertools import count
-    from pyrobolearn.simulators import BulletSim
+    from pyrobolearn.simulators import Bullet
     from pyrobolearn.worlds import BasicWorld
 
     # Create simulator
-    sim = BulletSim()
+    sim = Bullet()
 
     # create world
     world = BasicWorld(sim)
@@ -91,6 +91,8 @@ if __name__ == "__main__":
     robot.change_transparency()
     # robot.draw_link_coms()
     robot.draw_link_frames()
+    # robot.draw_joint_frames()
+    # robot.draw_joint_axes()
     # robot.draw_bounding_boxes(robot.right_leg[4])
 
     # run simulator

pyrobolearn/robots/robot.py

@@ -833,31 +833,31 @@ class Robot(ControllableBody):
         # set the joint torques
         self.set_joint_torques(torques, joint_ids)
 
-    def set_joint_torques(self, torque=None, joint_ids=None):
+    def set_joint_torques(self, torques=None, joint_ids=None):
         """
         Set the torque to the given joint(s) (using force/torque control).
 
         Args:
-            torque (float, np.array[N], None): desired torque(s) to apply to the joint(s) [N]. If None, it will apply
+            torques (float, np.array[N], None): desired torque(s) to apply to the joint(s) [N]. If None, it will apply
                 a torque of 0 to the given joint(s).
             joint_ids (int, int[N], None): joint id, or list of joint ids. If None, it will set the joint torques to
                 all (actuated) joints.
         """
         if isinstance(joint_ids, int):
-            if torque is None:
-                torque = 0
+            if torques is None:
+                torques = 0
         else:
             if joint_ids is None:
                 joint_ids = self.joints
             if not isinstance(joint_ids, collections.Iterable):
                 raise TypeError("Expecting jointId to be a tuple, list, or numpy array, got instead "
                                 "{}".format(type(joint_ids)))
-            if torque is None:
-                torque = [0]*len(joint_ids)
-            elif isinstance(torque, (int, float)):
-                torque = [torque]*len(joint_ids)
+            if torques is None:
+                torques = [0] * len(joint_ids)
+            elif isinstance(torques, (int, float)):
+                torques = [torques] * len(joint_ids)
 
-        self.sim.set_joint_motor_control(self.id, joint_ids, self.sim.TORQUE_CONTROL, forces=torque)
+        self.sim.set_joint_motor_control(self.id, joint_ids, self.sim.TORQUE_CONTROL, forces=torques)
 
     def set_joint_motor_control(self, joint_ids, control_mode, **kwargs):
         r"""
@@ -1090,6 +1090,43 @@ class Robot(ControllableBody):
         return self.sim.get_link_states(self.id, link_ids, compute_velocity=compute_link_velocity,
                                         compute_forward_kinematics=compute_forward_kinematics)
 
+    def get_link_local_position(self, link_ids=None):
+        """
+        Get the local position offset of the inertial frame (CoM) of the specified links expressed in the URDF link
+        frame.
+
+        Args:
+            link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the local position of all
+                links associated to actuated joints.
+
+        Returns:
+            if 1 link:
+                np.array[3]: local position offset of inertial frame (CoM) expressed in the URDF link frame
+            if multiple links: list of above
+        """
+        if isinstance(link_ids, int):
+            return self.get_link_states(self, link_ids, False, False)[2]
+        return [state[2] for state in self.get_link_states(self, link_ids, False, False)]
+
+    def get_link_local_orientations(self, link_ids=None):
+        """
+        Get the local orientation offset of the inertial frame (CoM) of the specified links expressed in the URDF link
+        frame.
+
+        Args:
+            link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the local position of all
+                links associated to actuated joints.
+
+        Returns:
+            if 1 link:
+                np.array[4]: local orientation (quaternion [x,y,z,w]) offset of inertial frame (CoM) expressed in the
+                    URDF link frame
+            if multiple links: list of above
+        """
+        if isinstance(link_ids, int):
+            return self.get_link_states(self, link_ids, False, False)[3]
+        return [state[3] for state in self.get_link_states(self, link_ids, False, False)]
+
     def get_link_names(self, link_ids=None):
         r"""
         Return the name of the given link(s).
@@ -1132,7 +1169,7 @@ class Robot(ControllableBody):
 
     def get_link_frames(self, link_ids=None, flatten=False):
         r"""
-        Return the link frame position and orientation (expressed in the world space).
+        Return the link world frame position(s) and orientation(s).
 
         Args:
             link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the frame position of all
@@ -1148,10 +1185,10 @@ class Robot(ControllableBody):
                 np.array[Nx4], np.array[N,4]: orientation of each link frame [x,y,z,w]
 
         """
-        return self.get_link_frame_world_positions(link_ids, flatten), self.get_link_frame_world_orientations(link_ids,
+        return self.get_link_world_frame_positions(link_ids, flatten), self.get_link_world_frame_orientations(link_ids,
                                                                                                               flatten)
 
-    def get_link_frame_world_positions(self, link_ids=None, flatten=False):
+    def get_link_world_frame_positions(self, link_ids=None, flatten=False):
         r"""
         Return the frame position (in the Cartesian world space coordinates) of the given link(s).
 
@@ -1175,7 +1212,7 @@ class Robot(ControllableBody):
             return pos.reshape(-1)  # 1D array
         return pos  # 2D array
 
-    def get_link_frame_world_orientations(self, link_ids=None, flatten=False):
+    def get_link_world_frame_orientations(self, link_ids=None, flatten=False):
         r"""
         Return the frame orientation (in the Cartesian world space) of the given link(s).
 
@@ -2730,7 +2767,7 @@ class Robot(ControllableBody):
         """
         joint_ids = self.joints if q_idx is None else self.joints[q_idx]
         torques = self.get_coriolis_and_gravity_compensation_torques(q, dq, q_idx)
-        self.set_joint_torques(torque=torques + external_torques, joint_ids=joint_ids)
+        self.set_joint_torques(torques=torques + external_torques, joint_ids=joint_ids)
 
     # TODO: finish to implement the method + think about multiple links + think about dimensions
     def get_active_compliant_torques(self, q=None, dq=None, q_idx=None, jacobian=None, link_velocity=None,
@@ -3455,6 +3492,8 @@ class Robot(ControllableBody):
     # online plotting  # # WARNING: ALL THE FOLLOWING METHODS NEED A SIMULATOR IN WHICH TO RUN #
     ####################
 
+    # TODO: move these functions elsewhere
+
     def plot_joint_positions(self, joint_ids=None):
         pass
 
@@ -3486,12 +3525,14 @@ class Robot(ControllableBody):
     # draw # # WARNING: ALL THE FOLLOWING METHODS NEED A SIMULATOR IN WHICH TO RUN #
     ########
 
+    # TODO: move these functions elsewhere
+
     def _draw_sphere(self, position, radius=0.1, color=(1, 1, 1, 1)):
         visual = self.sim.create_visual_shape(self.sim.GEOM_SPHERE, radius=radius, rgba_color=color)
         body = self.sim.create_body(mass=0, visual_shape_id=visual, position=position)
         return body
 
-    def _draw_cylinder(self, position, orientation, radius=1, height=1, color=(1, 1, 1, 1)):
+    def _draw_cylinder(self, position, orientation, radius=1., height=1., color=(1, 1, 1, 1)):
         visual = self.sim.create_visual_shape(self.sim.GEOM_CYLINDER, radius=radius, length=height, rgba_color=color)
         body = self.sim.create_body(mass=0., visual_shape_id=visual, position=position, orientation=orientation)
         return body
@@ -3641,7 +3682,7 @@ class Robot(ControllableBody):
             self.sim.remove_body(self.projected_com_visual)
             self.projected_com_visual = None
 
-    # def drawProjectedCoM(self, radius=0.05, color=(1,0,0,1)):
+    # def draw_projected_com(self, radius=0.05, color=(1,0,0,1)):
     #     """
     #     draw the projected CoM on the walking surface
     #     """
@@ -3684,25 +3725,80 @@ class Robot(ControllableBody):
             link_ids = [link_ids]
 
         for link in link_ids:
-            if link in self.visual_shapes:
-                if link == -1:
-                    pos, orientation = self.get_base_pose()
-                else:
-                    pos = self.get_link_frame_world_positions(link)
-                    orientation = self.get_link_frame_world_orientations(link)
-                dim = self.visual_shapes[link]['dimensions']
-                # radius = min(dim) * scaling * 0.2
-                radius = 0.005 * scaling
-                # self._draw_sphere(pos, radius, color=(0,0,0,1))
-                # length = 4*radius
-                length = 0.05 * scaling
-                self._draw_frame(pos, orientation, radius, length)
+            # if link in self.visual_shapes:
+            if link == -1:
+                position, orientation = self.get_base_pose()
+            else:
+                # position = self.get_link_world_frame_positions(link)
+                # orientation = self.get_link_world_frame_orientations(link)
+                position = self.get_link_world_positions(link)
+                orientation = self.get_link_world_orientations(link)
 
-    def draw_joint_frames(self, joint_ids=None):
+            # dim = self.visual_shapes[link]['dimensions']
+            # radius = min(dim) * scaling * 0.2
+            radius = 0.005 * scaling
+            # self._draw_sphere(position, radius, color=(0,0,0,1))
+            # length = 4*radius
+            length = 0.05 * scaling
+            self._draw_frame(position, orientation, radius, length)
+
+    def draw_joint_frames(self, joint_ids=None, scaling=1.):
         """
-        Draw (actuated) joint frame
+        Draw the specified actuated joint frames.
+
+        Args:
+            joint_ids (int, int[N], None): joint id, or list of joint ids. If None, draw all the actuated joint frames.
+            scaling (float): scaling factor
         """
-        pass
+        # check joints argument
+        if joint_ids is None:
+            joint_ids = self.joints
+        elif isinstance(joint_ids, int):
+            joint_ids = [joint_ids]
+        elif not isinstance(joint_ids, collections.Iterable):
+            raise TypeError("Expecting the given 'joint_ids' to be None, an int, or a list of int, instead got: "
+                            "{}".format(joint_ids))
+
+        positions = self.get_link_world_frame_positions(joint_ids)
+        orientations = self.get_link_world_frame_orientations(joint_ids)
+
+        # draw each joint axis
+        for joint, position, orientation in zip(joint_ids, positions, orientations):
+            radius = 0.005 * scaling
+            length = 0.05 * scaling
+            self._draw_frame(position, orientation, radius, length)
+
+    def draw_joint_axes(self, joint_ids=None, scaling=1.):
+        """
+        Draw the specified actuated joint axes.
+
+        Args:
+            joint_ids (int, int[N], None): joint id, or list of joint ids. If None, draw all the actuated joint axes.
+            scaling (float): scaling factor
+        """
+        # check joints argument
+        if joint_ids is None:
+            joint_ids = self.joints
+        elif isinstance(joint_ids, int):
+            joint_ids = [joint_ids]
+        elif not isinstance(joint_ids, collections.Iterable):
+            raise TypeError("Expecting the given 'joint_ids' to be None, an int, or a list of int, instead got: "
+                            "{}".format(joint_ids))
+
+        positions = self.get_link_world_frame_positions(joint_ids)
+        orientations = self.get_link_world_frame_orientations(joint_ids)
+        joint_axes = self.get_joint_axes(joint_ids)
+
+        # draw each joint axis
+        for joint, position, orientation, axis in zip(joint_ids, positions, orientations, joint_axes):
+            radius = 0.008 * scaling
+            length = 0.08 * scaling
+            R = get_matrix_from_quaternion(orientation)
+            y = R.dot(length / 2. * axis) + position
+            qx = np.array([0.707, 0, 0, 0.707])  # 90deg around x
+
+            # draw joint axis cylinder
+            self._draw_cylinder(y, get_quaternion_product(orientation, qx), radius, length, color=(1, 1, 0, 1))
 
     def draw_bounding_boxes(self, link_ids=None):
         """

pyrobolearn/robots/rrbot.py

@@ -44,10 +44,11 @@ class RRBot(ManipulatorRobot):
         self.name = 'rrbot'
 
         # set initial joint positions
-        self.set_joint_positions(self.joints, [np.pi / 4, np.pi / 2])
+        self.reset_joint_states(q=[np.pi / 4, np.pi / 2], joint_ids=self.joints)
+        # self.set_joint_positions(self.joints, [np.pi / 4, np.pi / 2])
 
-        for _ in range(100):
-            self.sim.step()
+        # for _ in range(100):
+        #     self.sim.step()
 
         # disable each motor joint
         self.disable_motor()
@@ -57,7 +58,7 @@ class RRBot(ManipulatorRobot):
         self.enable_joint_force_torque_sensor(2)
 
         # Coriolis and gravity compensation (note that the set_joint_torques need to be called at each time step)
-        self.enable_coriolis_and_gravity_compensation()
+        # self.enable_coriolis_and_gravity_compensation()
 
     def get_force_torque_sensor(self, idx=0):
         return np.array(self.sim.getJointState(self.id, 2)[2])