wassname/pyrobolearn
1
0
Fork 0
mirror of https://github.com/wassname/pyrobolearn.git synced 2026-09-09 11:31:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

add quaternion interpolation + PD feedback laws

Browse Source
This commit is contained in:
Brian Delhaisse
2019-06-25 02:02:48 +02:00
parent ef8ac14088
commit 542284fdde
3 changed files with 246 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pyrobolearn/utils/feedback.py
+157
View File
@@ -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
pyrobolearn/utils/interpolator.py
+2
View File
@@ -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"]
pyrobolearn/utils/transformation.py
+87 -14
View File
@@ -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