formatted

Author: Nicholas Nadeau

examples/trajectory_generation.ipynb

@@ -35,21 +35,22 @@
     "import matplotlib.pyplot as plt\n",
     "from mpl_toolkits.mplot3d import Axes3D\n",
     "\n",
+    "\n",
     "def plot_poses(p):\n",
     "    fig = plt.figure()\n",
     "    ax = fig.add_subplot(111, projection='3d')\n",
-    "    ax.plot(xs = p[:,0,-1], ys = p[:,1,-1],zs = p[:,2,-1],marker='o');\n",
+    "    ax.plot(xs=p[:, 0, -1], ys=p[:, 1, -1], zs=p[:, 2, -1], marker='o')\n",
     "\n",
-    "    ax.set_xlabel('X');\n",
-    "    ax.set_ylabel('Y');\n",
-    "    ax.set_zlabel('Z');\n",
+    "    ax.set_xlabel('X')\n",
+    "    ax.set_ylabel('Y')\n",
+    "    ax.set_zlabel('Z')\n",
     "\n",
-    "    ax.set_xlim(600,700);\n",
-    "    ax.set_ylim(-160,-140);\n",
-    "    ax.set_zlim(750,850);\n",
+    "    ax.set_xlim(600, 700)\n",
+    "    ax.set_ylim(-160, -140)\n",
+    "    ax.set_zlim(750, 850)\n",
     "\n",
-    "    plt.locator_params(nbins=4);\n",
-    "    plt.tight_layout();"
+    "    plt.locator_params(nbins=4)\n",
+    "    plt.tight_layout()"
    ]
   },
   {
@@ -60,11 +61,12 @@
    "source": [
     "def compute_deviation(p):\n",
     "    # compute deviation at joint-space midpoint\n",
-    "    p1 = p[0,:-1,-1]\n",
-    "    p2 = p[1,:-1,-1]\n",
-    "    p3 = p[2,:-1,-1]\n",
+    "    p1 = p[0, :-1, -1]\n",
+    "    p2 = p[1, :-1, -1]\n",
+    "    p3 = p[2, :-1, -1]\n",
     "\n",
-    "    deviation = np.linalg.norm(np.cross(p2-p1, p1-p3))/np.linalg.norm(p2-p1)\n",
+    "    deviation = np.linalg.norm(np.cross(p2 - p1,\n",
+    "                                        p1 - p3)) / np.linalg.norm(p2 - p1)\n",
     "\n",
     "    print(f'Deviation: {deviation:.5f}mm')"
    ]
@@ -82,8 +84,8 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "from pybotics.robot import Robot\n",
     "from pybotics.predefined_models import ur10\n",
+    "from pybotics.robot import Robot\n",
     "\n",
     "robot = Robot.from_parameters(ur10())"
    ]
@@ -109,8 +111,12 @@
     "from pybotics.geometry import vector_2_matrix\n",
     "\n",
     "poses = np.array([\n",
-    "    vector_2_matrix([600,-150,800,np.deg2rad(-90),0,np.deg2rad(-90)]),\n",
-    "    vector_2_matrix([700,-150,800,np.deg2rad(-90),0,np.deg2rad(-90)])\n",
+    "    vector_2_matrix([600, -150, 800,\n",
+    "                     np.deg2rad(-90), 0,\n",
+    "                     np.deg2rad(-90)]),\n",
+    "    vector_2_matrix([700, -150, 800,\n",
+    "                     np.deg2rad(-90), 0,\n",
+    "                     np.deg2rad(-90)])\n",
     "])\n",
     "\n",
     "start_end_joints = [robot.ik(p) for p in poses]"
@@ -160,10 +166,10 @@
     "# compute joint-space midpoint\n",
     "joints = [robot.ik(p, q=start_end_joints[0]) for p in poses]\n",
     "\n",
-    "mid_joints = np.mean(joints,axis=0)\n",
+    "mid_joints = np.mean(joints, axis=0)\n",
     "mid_pose = robot.fk(mid_joints)\n",
     "\n",
-    "deviated_poses = np.insert(arr=poses,obj=1,values=mid_pose,axis=0)"
+    "deviated_poses = np.insert(arr=poses, obj=1, values=mid_pose, axis=0)"
    ]
   },
   {
@@ -214,12 +220,12 @@
    "outputs": [],
    "source": [
     "# split linear trajectory in half, creating two joint-space segments\n",
-    "mid_cartesian_point = np.mean([poses[1,:-1,-1],poses[0,:-1,-1]],axis=0)\n",
+    "mid_cartesian_point = np.mean([poses[1, :-1, -1], poses[0, :-1, -1]], axis=0)\n",
     "\n",
     "mid_pose = poses[0].copy()\n",
-    "mid_pose[:-1,-1] = mid_cartesian_point\n",
+    "mid_pose[:-1, -1] = mid_cartesian_point\n",
     "\n",
-    "segmented_poses = np.insert(arr=poses,obj=1,values=mid_pose,axis=0)"
+    "segmented_poses = np.insert(arr=poses, obj=1, values=mid_pose, axis=0)"
    ]
   },
   {
@@ -260,18 +266,24 @@
    ],
    "source": [
     "# compute joints for new trajectory\n",
-    "joints = [robot.ik(p,q=start_end_joints[0]) for p in segmented_poses]\n",
+    "joints = [robot.ik(p, q=start_end_joints[0]) for p in segmented_poses]\n",
     "\n",
     "# compute deviation for first segment\n",
-    "mid_joints = np.mean(joints[:2],axis=0)\n",
+    "mid_joints = np.mean(joints[:2], axis=0)\n",
     "first_mid_pose = robot.fk(mid_joints)\n",
-    "first_segmented_poses = np.insert(arr=poses,obj=1,values=first_mid_pose,axis=0)\n",
+    "first_segmented_poses = np.insert(arr=poses,\n",
+    "                                  obj=1,\n",
+    "                                  values=first_mid_pose,\n",
+    "                                  axis=0)\n",
     "compute_deviation(first_segmented_poses)\n",
     "\n",
     "# compute deviation for second segment\n",
-    "mid_joints = np.mean(joints[1:],axis=0)\n",
+    "mid_joints = np.mean(joints[1:], axis=0)\n",
     "second_mid_pose = robot.fk(mid_joints)\n",
-    "second_segmented_poses = np.insert(arr=poses,obj=1,values=second_mid_pose,axis=0)\n",
+    "second_segmented_poses = np.insert(arr=poses,\n",
+    "                                   obj=1,\n",
+    "                                   values=second_mid_pose,\n",
+    "                                   axis=0)\n",
     "compute_deviation(second_segmented_poses)"
    ]
   }