diff --git a/pyrobolearn/utils/feedback.py b/pyrobolearn/utils/feedback.py
new file mode 100644
index 0000000..7eb641e
--- /dev/null
+++ b/pyrobolearn/utils/feedback.py
@@ -0,0 +1,157 @@
+#!/usr/bin/env python
+"""Provide feedback laws used in control.
+
+References:
+    [1] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010, chapter 2 and 3
+    [2] "Motion Planning and Control of Dynamic Humanoid Locomotion" (PhD thesis), Xin, 2018
+"""
+
+import numpy as np
+
+from pyrobolearn.utils.transformation import quaternion_error, vector_from_skew_matrix
+
+
+__author__ = ["Songyan Xin (code)", "Brian Delhaisse (documentation)"]
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Songyan Xin", "Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+def position_pd(pos_des, pos_cur, vel_des=np.zeros(3), vel_cur=np.zeros(3), acc_des=np.zeros(3), kp=100, kd=0.0):
+    r"""
+    Return the commanded spatial linear acceleration using the position PD feedback law given by:
+
+    .. math:: \ddot{p}_c = \ddot{p}_d + K_D (\dot{p}_d - \dot{p}) + K_P (p_d - p)
+
+    where :math:`\ddot{p}_c` is the commanded acceleration, :math:`\ddot{p}_d` is the desired acceleration, :math:`K_D`
+    is the derivative gain, :math:`\dot{p}_d` is the desired velocity, :math:`\dot{p}` is the current velocity,
+    :math:`K_P` is the proportional gain, :math:`p_d` is the desired position, and :math:`p` is the current position.
+
+    Args:
+        pos_des (np.array[3]): desired position
+        pos_cur (np.array[3]): current position
+        vel_des (np.array[3]): desired velocity
+        vel_cur (np.array[3]): current velocity
+        acc_des (np.array[3]): desired acceleration
+        kp (float, np.array[3,3]): proportional gain
+        kd (float, np.array[3,3]): derivative gain
+
+    Returns:
+        np.array[3]: the commanded spatial linear acceleration computed by the position PD feedback law
+    """
+    return kp * (pos_des - pos_cur) + kd * (vel_des - vel_cur) + acc_des
+
+
+def rotation_pd(rot_des, rot_cur, omega_des=np.zeros(3), omega_cur=np.zeros(3), omega_dot_des=np.zeros(3), kp=200,
+                kd=0.0):
+    r"""
+    Return the commanded spatial angular acceleration using the orientation PD feedback law given by:
+
+    .. math:: \dot{\omega}_c = \dot{\omega}_d + K_D (\omega_d - \omega) + K_P e_o
+
+    where :math:`\dot{\omega}_c` is the commanded angular acceleration, \dot{\omega}_d is the desired angular
+    acceleration, :math:`K_D` is the derivative gain, :math:`\omega` is the angular velocity, :math:`K_P` is the
+    proportional gain, and :math:`e_o` is the orientation error (which depends on the orientation representation we
+    are using).
+
+    If rotation matrices are given, the orientation error is given by:
+
+    .. math:: e_o = vex(R_d R^\top - I)
+
+    where :math:`vex` convert a skew-symmetric matrix to a vector (it is the inverse of the `skew(.)` function).
+
+    Args:
+        rot_des (np.array[3, 3]): desired rotation matrix
+        rot_cur (np.array[3, 3]): current rotation matrix
+        omega_des (np.array[3]): desired angular velocity
+        omega_cur (np.array[3]): current angular velocity
+        omega_dot_des (np.array[3]): desired angular acceleration
+        kp (float, np.array[3,3]): proportional gain
+        kd (float, np.array[3,3]): derivative gain
+
+    Returns:
+        np.array[3]: the commanded spatial angular acceleration computed by the orientation PD feedback law
+    """
+    rot_error = vector_from_skew_matrix(rot_des.dot(rot_cur.T) - np.identity(3))
+    return kp * rot_error + kd * (omega_des - omega_cur) + omega_dot_des
+
+
+def quaternion_pd(quat_des, quat_cur, omega_des=np.zeros(3), omega_cur=np.zeros(3), omega_dot_des=np.zeros(3), kp=100,
+                 kd=0.0):
+    r"""
+    Return the commanded spatial angular acceleration using the orientation PD feedback law given by:
+
+    .. math:: \dot{\omega}_c = \dot{\omega}_d + K_D (\omega_d - \omega) + K_P e_o
+
+    where :math:`\dot{\omega}_c` is the commanded angular acceleration, \dot{\omega}_d is the desired angular
+    acceleration, :math:`K_D` is the derivative gain, :math:`\omega` is the angular velocity, :math:`K_P` is the
+    proportional gain, and :math:`e_o` is the orientation error (which depends on the orientation representation we
+    are using).
+
+    If quaternions are given, the orientation error is given by:
+
+    .. math:: e_o = s v_d - s_d v - v_d \cross v
+
+    where a quaternion is represented as an ordered pair :math:`[s, v]` with :math:`s \in \mathbb{R}` (the scalar part)
+    and :math:`v \in \mathbb{R}^3` (the vector part).
+
+    Args:
+        quat_des (np.array[4]): desired quaternion [x,y,z,w]
+        quat_cur (np.array[4]): current quaternion [x,y,z,w]
+        omega_des (np.array[3]): desired angular velocity
+        omega_cur (np.array[3]): current angular velocity
+        omega_dot_des (np.array[3]): desired angular acceleration
+        kp (float, np.array[3,3]): proportional gain
+        kd (float, np.array[3,3]): derivative gain
+
+    Returns:
+        np.array[3]: the commanded spatial angular acceleration computed by the orientation PD feedback law
+    """
+    return kp * quaternion_error(quat_des=quat_des, quat_cur=quat_cur) + kd * (omega_des - omega_cur) + omega_dot_des
+
+
+def pose_pd(pose_des, pose_cur, spatial_velocity_des=np.zeros(6), spatial_velocity_cur=np.zeros(6),
+            spatial_acceleration_des=np.zeros(6), kp_linear=100, kd_linear=10, kp_angular=100, kd_angular=10):
+    r"""
+    Return the commanded spatial acceleration given by :math:`\dot{v}_c = [\dot{\omega}_c^\top, \ddot{p}_c^\top]^\top`,
+    computed using the position/orientation PD feedback laws:
+
+    .. math::
+
+        \dot{\omega}_c &= \dot{\omega}_d + K_{D,\omega} (\omega_d - \omega) + K_{P,\omega} e_o \\
+        \ddot{p}_c = \ddot{p}_d + K_{D,p} (\dot{p}_d - \dot{p}) + K_{P,p} (p_d - p),
+
+    with :math:`[p, q] \in \mathbb{R}^7` is the pose with :math:`p \in \mathbb{R}^3` being the position, and :math:`q`
+    being the quaternion, while :math:`v = [\omega, \dot{p}] \in \mathbb{R}^6` is the spatial velocity with
+    :math:`\omega \in \mathbb{R}^3` being the angular velocity and :math:`\dot{p} \in \mathbb{R}^3` being the linear
+    velocity.
+
+    Args:
+        pose_des (np.array[7]): desired pose :math:`[p_d, q_d]` which is the concatenation of the desired position and
+            orientation  (where the later is represented as a quaternion [x,y,z,w])
+        pose_cur (np.array[7]): current pose :math:`[p, q]` which is the concatenation of the current position and
+            orientation (where the latter is represented as a quaternion [x,y,z,w])
+        spatial_velocity_des (np.array[6]): desired spatial velocity :math:`v = [\omega, \dot{p}]`
+        spatial_velocity_cur (np.array[6]): current spatial velocity :math:`v = [\omega, \dot{p}]`
+        spatial_acceleration_des (np.array[6]): desired spatial acceleration :math:`\dot{v} = [\dot{\omega}, \ddot{p}]`
+        kp_linear (float, np.array[3,3]): linear proportional gain
+        kd_linear (float, np.array[3,3]): linear derivative gain
+        kp_angular (float, np.array[3,3]): angular proportional gain
+        kd_angular (float, np.array[3,3]): angular derivative gain
+
+    Returns:
+        np.array[6]: commanded spatial acceleration computed by the PD feedback law (that is the error computed by the
+            PD feedback laws)
+    """
+    error_linear = position_pd(pos_cur=pose_cur[:3], pos_des=pose_des[:3], vel_cur=spatial_velocity_cur[-3:],
+                               vel_des=spatial_velocity_des[-3:], acc_des=spatial_acceleration_des[-3:],
+                               kp=kp_linear, kd=kd_linear)
+    error_angular = quaternion_pd(quat_cur=pose_cur[-4:], quat_des=pose_des[-4:], omega_cur=spatial_velocity_cur[:3],
+                                  omega_des=spatial_velocity_des[:3], omega_dot_des=spatial_acceleration_des[:3],
+                                  kp=kp_angular, kd=kd_angular)
+    error = np.concatenate((error_angular, error_linear))
+    return error
diff --git a/pyrobolearn/utils/interpolator.py b/pyrobolearn/utils/interpolator.py
index 7199e4f..a225a90 100644
--- a/pyrobolearn/utils/interpolator.py
+++ b/pyrobolearn/utils/interpolator.py
@@ -4,6 +4,8 @@
 
 import numpy as np
 
+from pyrobolearn.utils.transformation import slerp, squad
+
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
diff --git a/pyrobolearn/utils/transformation.py b/pyrobolearn/utils/transformation.py
index 309c1d0..1f4dd33 100644
--- a/pyrobolearn/utils/transformation.py
+++ b/pyrobolearn/utils/transformation.py
@@ -10,6 +10,7 @@ References:
 
 import numpy as np
 import quaternion
+# from pyquaternion import Quaternion  # TODO: check API at http://kieranwynn.github.io/pyquaternion
 import sympy
 from collections import Iterable
 
@@ -610,6 +611,10 @@ def skew_matrix(vector):
                      [-y, x, 0.]])
 
 
+# alias (for people who use http://www.petercorke.com/RTB/r9/html/skew.html)
+skew = skew_matrix
+
+
 def vector_from_skew_matrix(matrix):
     r"""
     Convert skew-symmetric matrix to vector; this is the inverse of the function `skew_matrix`.
@@ -631,6 +636,10 @@ def vector_from_skew_matrix(matrix):
                            matrix[1, 0] - matrix[0, 1]])
 
 
+# alias (for people who use http://www.petercorke.com/RTB/r9/html/vex.html)
+vex = vector_from_skew_matrix
+
+
 def rotation_matrix_x(angle):
     """
     Return the rotation matrix around the x-axis by the given angle.
@@ -838,15 +847,17 @@ def get_quaternion_product(q1, q2, convention='xyzw'):
 
 
 def quaternion_error(quat_des, quat_cur):
-    """
-    Quaternion error between two quaternions.
+    r"""
+    Compute the orientation (vector) error between the current and desired quaternion; that is, it is the difference
+    between :math:`q_curr` and :math:`q_des`, which is given by: :math:`\Delta q = q_{curr}^{-1} q_{des}`.
+    Only the vector part is returned which can be used in PD control.
 
     Args:
-        quat_des (np.array[4], quaternion.quaternion): desired quaternion
-        quat_cur (np.array[4], quaternion.quaternion): current quaternion
+        quat_des (np.array[4]): desired quaternion [x,y,z,w]
+        quat_cur (np.array[4]): current quaternion [x,y,z,w]
 
     Returns:
-
+        np.array[3]: vector error between the current and desired quaternion
     """
     diff = quat_cur[-1] * quat_des[:3] - quat_des[-1] * quat_cur[:3] - skew_matrix(quat_des[:3]).dot(quat_cur[:3])
     return diff
@@ -905,35 +916,97 @@ def angular_velocity_from_quaternion(q1, q2):
     return 2 * logarithm_map(q1 * q2)
 
 
-def slerp(q0, qf):
+def slerp(q0, qf, t, t0=0., tf=1.):
     """
     Interpolate between two quaternions using Spherical Linear intERPolation (SLERP).
 
     Args:
-        q1:
-        q2:
+        q0 (np.array[4], quaternion.quaternion): initial quaternion.
+        qf (np.array[4], quaternion.quaternion): final quaternion.
+        t (float, list of float, np.array): the times to which the quaternions should be interpolated.
+        t0 (float): initial time corresponding to the initial quaternion.
+        tf (float): final time corresponding to the final quaternion.
 
     Returns:
+        np.array, quaternion, np.array of quaternion: one or multiple interpolated quaternions
 
     References:
-        [1] "Understanding Quaternions", https://www.3dgep.com/understanding-quaternions
+        - [1] "Understanding Quaternions", https://www.3dgep.com/understanding-quaternions
+        - [2] Documentation of numpy-quaternion
     """
-    pass
+    # convert if necessary
+    is_input_quaternion = True
+    if not isinstance(q0, quaternion.quaternion):
+        q0 = quaternion.quaternion(q0[3], q0[0], q0[1], q0[2])
+        is_input_quaternion = False
+    if not isinstance(qf, quaternion.quaternion):
+        qf = quaternion.quaternion(qf[3], qf[0], qf[1], qf[2])
+
+    t = np.asarray(t)
+
+    # interpolate using quaternion library
+    qs = quaternion.slerp(q0, qf, t0, tf, t)
+
+    # if we need to convert back to np.array
+    if not is_input_quaternion:
+        if isinstance(qs, np.ndarray):
+            qs = quaternion.as_float_array(qs)
+            qs = np.hstack((qs[:, 1:], qs[:, 0, np.newaxis]))
+            return qs
+        return np.array([qs.x, qs.y, qs.z, qs.w])
+    return qs
 
 
-def squad(quaternions):
+def squad(quaternions, times, t):
     r"""
     Smoothly interpolate over a list/path of rotations using Spherical and QUADrangle (SQUAD).
 
+    From [2]: "Spherical "quadrangular" interpolation of rotors with a cubic spline
+
+    This is the best way to interpolate rotations.  It uses the analog of a cubic spline, except that the interpolant
+    is confined to the rotor manifold in a natural way.  Alternative methods involving interpolation of other
+    coordinates on the rotation group or normalization of interpolated values give bad results. The results from this
+    method are as natural as any, and are continuous in first and second derivatives.
+
+    The input `R_in` rotors are assumed to be reasonably continuous (no sign flips), and the input `t` arrays are
+    assumed to be sorted. No checking is done for either case, and you may get silently bad results if these
+    conditions are violated."
+
     Args:
-        quaternions (list of np.array, list of quaternion.quaternion):
+        quaternions (list of np.array[4], list of quaternion.quaternion): A time-series of rotors (unit quaternions)
+            to be interpolated
+        times (np.array, list of float): the times corresponding to the quaternions.
+        t (np.array, list of float): the times to which the quaternions should be interpolated.
 
     Returns:
+        np.array, np.array of quaternion: interpolated quaternions
 
     References:
-        [1] "Understanding Quaternions", https://www.3dgep.com/understanding-quaternions
+        - [1] "Understanding Quaternions", https://www.3dgep.com/understanding-quaternions
+        - [2] Documentation of numpy-quaternion
     """
-    pass
+    # make sure that they are numpy arrays
+    quaternions = np.asarray(quaternions)
+    times = np.asarray(times)
+    t = np.asarray(t)
+
+    # check if we need to convert
+    are_input_quaternions = (quaternions.dtype == quaternion.quaternion)
+    if not are_input_quaternions:
+        qs = quaternions
+        quaternions = quaternion.from_float_array(np.hstack((qs[:, 0, np.newaxis], qs[:, 1:])))
+
+    # interpolate
+    qs = quaternion.squad(quaternions, times, t)
+
+    # if we need to convert back to np.array
+    if not are_input_quaternions:
+        if isinstance(qs, np.ndarray):
+            qs = quaternion.as_float_array(qs)
+            qs = np.hstack((qs[:, 1:], qs[:, 0, np.newaxis]))
+            return qs
+        return np.array([qs.x, qs.y, qs.z, qs.w])
+    return qs
 
 
 # Tests