Example_1_mr.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import modern_robotics as mr"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "['Adjoint',\n",
       " 'AxisAng3',\n",
       " 'AxisAng6',\n",
       " 'CartesianTrajectory',\n",
       " 'ComputedTorque',\n",
       " 'CubicTimeScaling',\n",
       " 'DistanceToSE3',\n",
       " 'DistanceToSO3',\n",
       " 'EndEffectorForces',\n",
       " 'EulerStep',\n",
       " 'FKinBody',\n",
       " 'FKinSpace',\n",
       " 'ForwardDynamics',\n",
       " 'ForwardDynamicsTrajectory',\n",
       " 'GravityForces',\n",
       " 'IKinBody',\n",
       " 'IKinSpace',\n",
       " 'InverseDynamics',\n",
       " 'InverseDynamicsTrajectory',\n",
       " 'JacobianBody',\n",
       " 'JacobianSpace',\n",
       " 'JointTrajectory',\n",
       " 'MassMatrix',\n",
       " 'MatrixExp3',\n",
       " 'MatrixExp6',\n",
       " 'MatrixLog3',\n",
       " 'MatrixLog6',\n",
       " 'NearZero',\n",
       " 'Normalize',\n",
       " 'ProjectToSE3',\n",
       " 'ProjectToSO3',\n",
       " 'QuinticTimeScaling',\n",
       " 'RotInv',\n",
       " 'RpToTrans',\n",
       " 'ScrewToAxis',\n",
       " 'ScrewTrajectory',\n",
       " 'SimulateControl',\n",
       " 'TestIfSE3',\n",
       " 'TestIfSO3',\n",
       " 'TransInv',\n",
       " 'TransToRp',\n",
       " 'VecTose3',\n",
       " 'VecToso3',\n",
       " 'VelQuadraticForces',\n",
       " '__builtins__',\n",
       " '__cached__',\n",
       " '__doc__',\n",
       " '__file__',\n",
       " '__loader__',\n",
       " '__name__',\n",
       " '__package__',\n",
       " '__path__',\n",
       " '__spec__',\n",
       " '__version__',\n",
       " 'ad',\n",
       " 'core',\n",
       " 'np',\n",
       " 'print_function',\n",
       " 'se3ToVec',\n",
       " 'so3ToVec']"
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "dir(mr)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([0.        , 0.        , 0.78539816])"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# unit rotation axis\n",
    "omega_hat = mr.np.array([0,0,1]) \n",
    "theta = mr.np.pi/4\n",
    "# 3-vector form  of rotation angle\n",
    "omega = omega_hat*theta \n",
    "omega"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Help on function AxisAng3 in module modern_robotics.core:\n",
      "\n",
      "AxisAng3(expc3)\n",
      "    Converts a 3-vector of exponential coordinates for rotation into\n",
      "    axis-angle form\n",
      "    \n",
      "    :param expc3: A 3-vector of exponential coordinates for rotation\n",
      "    :return omghat: A unit rotation axis\n",
      "    :return theta: The corresponding rotation angle\n",
      "    \n",
      "    Example Input:\n",
      "        expc3 = np.array([1, 2, 3])\n",
      "    Output:\n",
      "        (np.array([0.26726124, 0.53452248, 0.80178373]), 3.7416573867739413)\n",
      "\n"
     ]
    }
   ],
   "source": [
    "help(mr.AxisAng3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(array([0., 0., 1.]), 0.7853981633974483)"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# get Unit axis of Rotation and theta from 3-vector of exponential coordinates \n",
    "mr.AxisAng3(omega) "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Help on function AxisAng3 in module modern_robotics.core:\n",
      "\n",
      "AxisAng3(expc3)\n",
      "    Converts a 3-vector of exponential coordinates for rotation into\n",
      "    axis-angle form\n",
      "    \n",
      "    :param expc3: A 3-vector of exponential coordinates for rotation\n",
      "    :return omghat: A unit rotation axis\n",
      "    :return theta: The corresponding rotation angle\n",
      "    \n",
      "    Example Input:\n",
      "        expc3 = np.array([1, 2, 3])\n",
      "    Output:\n",
      "        (np.array([0.26726124, 0.53452248, 0.80178373]), 3.7416573867739413)\n",
      "\n"
     ]
    }
   ],
   "source": [
    "help(mr.AxisAng3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.        , -0.78539816,  0.        ],\n",
       "       [ 0.78539816,  0.        , -0.        ],\n",
       "       [-0.        ,  0.        ,  0.        ]])"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# get so3(skew-symmetric matrix) from 3-vector of exponential coordinates \n",
    "so3 = mr.VecToso3(omega)\n",
    "so3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([0.        , 0.        , 0.78539816])"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# get 3-vector of exponential coordinates from so3(skew-symmetric matrix)\n",
    "omega = mr.so3ToVec(so3)\n",
    "omega"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.70710678, -0.70710678,  0.        ],\n",
       "       [ 0.70710678,  0.70710678,  0.        ],\n",
       "       [ 0.        ,  0.        ,  1.        ]])"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# get SO3(Rotation matrix) from so3(skew-symmetric matrix)\n",
    "R = mr.MatrixExp3(so3)\n",
    "R"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.        , -0.78539816,  0.        ],\n",
       "       [ 0.78539816,  0.        ,  0.        ],\n",
       "       [ 0.        ,  0.        ,  0.        ]])"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# get so3(skew-symmetric matrix) from SO3(Rotation matrix)\n",
    "so3 = mr.MatrixLog3(R)\n",
    "so3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([1, 0, 0, 1, 2, 3])"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# 6-vector form twist, V \n",
    "V = mr.np.array([1,0,0,1,2,3])\n",
    "V"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.,  0.,  0.,  1.],\n",
       "       [ 0.,  0., -1.,  2.],\n",
       "       [ 0.,  1.,  0.,  3.],\n",
       "       [ 0.,  0.,  0.,  0.]])"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# get se3(twist 4x4 matrix) from 6-vector spatial velocity, V\n",
    "se3 = mr.VecTose3(V)\n",
    "se3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(array([1., 0., 0., 1., 2., 3.]), 1.0)"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "S, theta_dot = mr.AxisAng6(V)\n",
    "S, theta_dot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([1., 0., 0., 1., 2., 3.])"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# get 6-vector spatial velocity, V from se3(twist 4x4 matrix)\n",
    "V = mr.se3ToVec(se3)\n",
    "V"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 1.        ,  0.        ,  0.        ,  1.        ],\n",
       "       [ 0.        ,  0.54030231, -0.84147098,  0.30384889],\n",
       "       [ 0.        ,  0.84147098,  0.54030231,  3.44380834],\n",
       "       [ 0.        ,  0.        ,  0.        ,  1.        ]])"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# get Homogeneous Transformation matrix T from se3(twist 4x4 matrix)\n",
    "T = mr.MatrixExp6(se3)\n",
    "T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.,  0.,  0.,  1.],\n",
       "       [ 0.,  0., -1.,  2.],\n",
       "       [ 0.,  1.,  0.,  3.],\n",
       "       [ 0.,  0.,  0.,  0.]])"
      ]
     },
     "execution_count": 16,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# get se3(twist 4x4 matrix) from Homogeneous Transformation matrix, T\n",
    "se3 = mr.MatrixLog6(T)\n",
    "se3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([1., 0., 0., 1., 2., 3.])"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "V = mr.se3ToVec(se3)\n",
    "V"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}