nnadeau/pybotics

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Robot Dynamics\n",
    "\n",
    "- How is the motion of a manipulator affected by external forces?\n",
    "- What joints torques are generated from external forces applied to the manipulator?\n",
    "- The following example will demonstrate how to calculate the joint torques required to counteract a given TCP wrench."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Create the Robot Model"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "from pybotics.robot import Robot\n",
    "from pybotics.predefined_models import ur10\n",
    "\n",
    "robot = Robot.from_parameters(ur10())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Define the Forces/Torques Acting on the TCP\n",
    "\n",
    "- Aka the \"wrench\""
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "forces = [0, 0, 10]\n",
    "torques = [0, 0, 0]\n",
    "wrench = [*forces, *torques]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Calculate Joint Torques\n",
    "\n",
    "- What are the joint torques required to counteract this payload?\n",
    "- This calculation can be repeated at each discrete pose in a trajectory for trajectory dynamics"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Robot Joints: [0. 0. 0. 0. 0. 0.]\n",
      "Joint Torques: [11843.     0.     0.     0.     0.     0.]\n",
      "Robot Joints: [ 0.         -1.57079633 -1.57079633  0.         -1.57079633  0.        ]\n",
      "Joint Torques: [-1639.  7284.  1157.  1157.     0.     0.]\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "\n",
    "robot.joints = np.deg2rad([0, 0, 0, 0, 0, 0])\n",
    "j_torques = robot.compute_joint_torques(wrench)\n",
    "print(f'Robot Joints: {robot.joints}')\n",
    "print(f'Joint Torques: {j_torques}')\n",
    "\n",
    "robot.joints = np.deg2rad([0, -90, -90, 0, -90, 0])\n",
    "j_torques = robot.compute_joint_torques(wrench)\n",
    "print(f'Robot Joints: {robot.joints}')\n",
    "print(f'Joint Torques: {j_torques}')"
   ]
  }
 ],
 "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.7.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}