erratum_bodyrate_transform.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true,
    "ExecuteTime": {
     "start_time": "2023-07-23T19:44:29.993618Z",
     "end_time": "2023-07-23T19:44:32.695825Z"
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "J_omega2euler_wrong: \n",
      " Matrix([[c2, 0, s2], [0, 1, 0], [-c1*s2, 1.0*s1, c1*c2]])\n",
      "J_omega2euler_liegroup: \n",
      " Matrix([[1.0*c2, 0, 1.0*s2], [1.0*s2*tan(q1(t)), 1, -1.0*c2*tan(q1(t))], [-1.0*s2/c1, 0, 1.0*c2/c1]])\n",
      "J_omega2euler_new: \n",
      " Matrix([[c2, 0, s2], [1.0*s2*tan(q1(t)), 1, -1.0*c2*tan(q1(t))], [-s2/c1, 0, c2/c1]])\n"
     ]
    }
   ],
   "source": [
    "import sympy as sp\n",
    "sp.init_printing(use_unicode=True)\n",
    "\n",
    "t  = sp.symbols('t')\n",
    "q1, q2, q3 = sp.Function('q1'), sp.Function('q2'), sp.Function('q3')\n",
    "s1, s2, s3 = sp.sin(q1(t)), sp.sin(q2(t)), sp.sin(q3(t))\n",
    "c1, c2, c3 = sp.cos(q1(t)), sp.cos(q2(t)), sp.cos(q3(t))\n",
    "e1, e2, e3 = sp.Matrix([1, 0, 0]), sp.Matrix([0, 1, 0]), sp.Matrix([0, 0, 1])\n",
    "\n",
    "# Rotations\n",
    "# See \"Dynamic modeling and characteristics analysis of lateral-pendulum unicycle robot\" by Daud et al\n",
    "# (The columns of the rotation matrix are the wheel frame unit vectors expressed contact point frame)\n",
    "\n",
    "# Rotation from wheel frame to contact point frame\n",
    "R1 = sp.Matrix([[1, 0., 0.],\n",
    "                [0., c1, -s1],\n",
    "                [0., s1, c1]])\n",
    "\n",
    "# Rotation from body frame to wheel frame\n",
    "R2 = sp.Matrix([[c2, 0., s2],\n",
    "                [0., 1., 0.],\n",
    "                [-s2, 0., c2]])\n",
    "\n",
    "# Rotation from contact point frame to inertial frame\n",
    "R3 = sp.Matrix([[c3, -s3, 0.],\n",
    "                [s3, c3, 0.],\n",
    "                [0., 0., 1.]])\n",
    "\n",
    "#R_BI = R2.T * R1.T * R3.T\n",
    "R_IB = R3 * R1 * R2 # Rotation of B frame expressed in I frame\n",
    "\n",
    "dR_IB = sp.diff(R_IB, t)\n",
    "\n",
    "\"\"\"\n",
    "The formula below expresses the angular velocity \"omega\" relative to the inertia frame in the body frame.\n",
    "See:\n",
    "- Page 78, Lynch - \"Modern Robotics\"\n",
    "- Page 3, Kim - \"Lie Group Formulation of Articulated Rigid Body Dynamics\"\n",
    "\"\"\"\n",
    "omega_B_mat = R_IB.T * dR_IB\n",
    "\n",
    "def so3toVec(R):\n",
    "    \"\"\"\n",
    "    Converts an so(3) matrix to vec using the Schur complement\n",
    "    Source: Modern Robotics - https://github.com/NxRLab/ModernRobotics\n",
    "    \"\"\"\n",
    "    return sp.Matrix([R[2, 1], R[0, 2], R[1, 0]])\n",
    "\n",
    "omega_B_vec = sp.simplify(so3toVec(omega_B_mat))\n",
    "\n",
    "col1 = omega_B_vec.subs([(sp.Derivative(q1(t), t), 1), (sp.Derivative(q2(t), t), 0), (sp.Derivative(q3(t), t), 0)])\n",
    "col2 = omega_B_vec.subs([(sp.Derivative(q1(t), t), 0), (sp.Derivative(q2(t), t), 1), (sp.Derivative(q3(t), t), 0)])\n",
    "col3 = omega_B_vec.subs([(sp.Derivative(q1(t), t), 0), (sp.Derivative(q2(t), t), 0), (sp.Derivative(q3(t), t), 1)])\n",
    "J_euler2omega = sp.simplify(col1.row_join(col2).row_join(col3))\n",
    "J_omega2euler_liegroup = sp.simplify(J_euler2omega.inv())\n",
    "\n",
    "# Expression from the paper\n",
    "J_omega2euler_wrong=  (e1.T * R2).col_join(e2.T).col_join(e3.T*R1*R2)\n",
    "\n",
    "# I again did the derivation by hand...\n",
    "# Note:\n",
    "# - dq2 points along the rotation axis between the b-frame and w-frame (second axis), so it is already in the b frame\n",
    "# - dq1 points along the rotation axis between the w-frame and cp-frame (first axis), so it is transformed from w-frame to b-frame\n",
    "# - dq3 points along the rotation axis between the cp-frame and inertial frame (third axis), so it is transformed from cp-frame to b-frame\n",
    "J_euler2omega_new =  sp.Matrix.hstack(R2.T*e1, e2, R2.T*R1.T*e3)\n",
    "J_omega2euler_new = sp.simplify(J_euler2omega_new.inv()) # <- HERE IS THE MISTAKE, I DIDN'T INVERT THE MATRIX BUT TRANSPOSED IT\n",
    "\n",
    "def make_rot_readable(R):\n",
    "    return R.subs([(c1, 'c1'), (c2, 'c2'), (c3, 'c3'), (s1, 's1'), (s2, 's2'), (s3, 's3'),\n",
    "                   (sp.sin(2*q1(t)), 2*s1*c1), (sp.sin(2*q2(t)), 2*s2*c2), (sp.sin(2*q3(t)), 2*s3*c3)])\n",
    "\n",
    "J_omega2euler_wrong= make_rot_readable(J_omega2euler_wrong)\n",
    "J_omega2euler_liegroup = make_rot_readable(J_omega2euler_liegroup)\n",
    "J_omega2euler_new = make_rot_readable(J_omega2euler_new)\n",
    "\n",
    "print(f'J_omega2euler_wrong: \\n {J_omega2euler_wrong}')\n",
    "print(f'J_omega2euler_liegroup: \\n {J_omega2euler_liegroup}')\n",
    "print(f'J_omega2euler_new: \\n {J_omega2euler_new}')\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "outputs": [],
   "source": [],
   "metadata": {
    "collapsed": false
   }
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}