{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import cadquery as cq\n",
    "from jupyter_cadquery import show, set_defaults, open_viewer, close_viewer, close_viewers\n",
    "from cadquery_massembly import Mate, MAssembly, relocate\n",
    "\n",
    "cv = open_viewer(\"Linkage\", cad_width=640, height=500, theme=\"light\")\n",
    "\n",
    "# bypass \"clean\" to avoid errors OCP kernel error\n",
    "cq.occ_impl.shapes.Shape.clean = lambda x: x"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "set_defaults(axes=False, axes0=True, mate_scale=2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Jansen Linkage "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![3-jansen-linkage.png](3-jansen-linkage.png)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Model"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "import numpy as np\n",
    "\n",
    "Vec = lambda x,y: np.array((x, y))\n",
    "\n",
    "def intersect(p0, r0, p1, r1):\n",
    "    \"\"\"\n",
    "    Bourke's algorithm (http://paulbourke.net/geometry/circlesphere)\n",
    "    to find intersect points of circle0 (p0, r0) and circle1 (p1, r1)\n",
    "    \"\"\"\n",
    "    p10 = p1 - p0\n",
    "    d = np.linalg.norm(p10)\n",
    "    if (d > r0 + r1) or (d < abs(r1 - r0)) or ((d == 0) and (r0 == r1)):\n",
    "        return None\n",
    "    \n",
    "    a = (r0**2 - r1**2 + d**2) / (2 * d)\n",
    "    h = np.sqrt(r0**2 - a**2)\n",
    "    p2 = p0 + (a / d) * p10\n",
    "    r = Vec(-p10[1], p10[0]) * (h / d)\n",
    "\n",
    "    return (p2 - r, p2 + r)\n",
    "\n",
    "\n",
    "def link_loc(name, joints, links):\n",
    "    p1_index, p2_index = name.split(\"_\")[1:]\n",
    "    p1 = joints[int(p1_index)]\n",
    "    p2 = joints[int(p2_index)]\n",
    "    a = math.degrees(math.atan2(p1[1] - p2[1], p1[0] - p2[0]))\n",
    "    return (np.array((links[name][\"lev\"], *p1)), a)\n",
    "\n",
    "    \n",
    "def linkage(alpha, x, y, links):\n",
    "    \"\"\"For a given angle return the 2d location of each joint\"\"\"\n",
    "    p0 = Vec(0, 0)\n",
    "    p1 = Vec(x, y)\n",
    "    p2 = p1 + links[\"link_1_2\"][\"len\"] * Vec(np.cos(np.deg2rad(alpha)), np.sin(np.deg2rad(alpha)))\n",
    "    p3 = intersect(p0, links[\"link_0_3\"][\"len\"], p2, links[\"link_2_3\"][\"len\"])[1]\n",
    "    p4 = intersect(p0, links[\"link_4_0\"][\"len\"], p3, links[\"link_3_4\"][\"len\"])[1]\n",
    "    p5 = intersect(p0, links[\"link_0_5\"][\"len\"], p2, links[\"link_2_5\"][\"len\"])[0]\n",
    "    p6 = intersect(p4, links[\"link_4_6\"][\"len\"], p5, links[\"link_5_6\"][\"len\"])[0]\n",
    "    p7 = intersect(p5, links[\"link_7_5\"][\"len\"], p6, links[\"link_6_7\"][\"len\"])[1]\n",
    "    return (p0, p1, p2, p3, p4, p5, p6, p7)\n",
    "\n",
    "height = 2\n",
    "x = 38.0\n",
    "y =  7.8\n",
    "\n",
    "links = {}\n",
    "links[\"link_1_2\"] = {\"len\": 15.0, \"lev\": 3 * height, \"col\": \"DarkBlue\"}\n",
    "links[\"link_2_3\"] = {\"len\": 50.0, \"lev\": 4 * height, \"col\": \"DarkGreen\"}\n",
    "links[\"link_3_4\"] = {\"len\": 55.8, \"lev\": 3 * height, \"col\": \"Red\"}\n",
    "links[\"link_4_0\"] = {\"len\": 40.1, \"lev\": 1 * height, \"col\": \"Red\"}\n",
    "links[\"link_0_3\"] = {\"len\": 41.5, \"lev\": 2 * height, \"col\": \"Red\"}\n",
    "links[\"link_4_6\"] = {\"len\": 39.4, \"lev\": 2 * height, \"col\": \"Purple\"}\n",
    "links[\"link_0_5\"] = {\"len\": 39.3, \"lev\": 3 * height, \"col\": \"OliveDrab\"}\n",
    "links[\"link_2_5\"] = {\"len\": 61.9, \"lev\": 1 * height, \"col\": \"Orange\"}\n",
    "links[\"link_5_6\"] = {\"len\": 36.7, \"lev\": 0 * height, \"col\": \"RoyalBlue\"}\n",
    "links[\"link_6_7\"] = {\"len\": 65.7, \"lev\": 1 * height, \"col\": \"RoyalBlue\"}\n",
    "links[\"link_7_5\"] = {\"len\": 49.0, \"lev\": 2 * height, \"col\": \"RoyalBlue\"}\n",
    "\n",
    "link_list = list(links.keys())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Visualisation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "import matplotlib.gridspec as gridspec\n",
    "%matplotlib inline\n",
    "\n",
    "def c(a,b):\n",
    "    return links[f\"link_{a}_{b}\"][\"col\"].replace(\"Blue4\", \"blue\")\n",
    "\n",
    "def plot(ax, joints):\n",
    "    p0, p1, p2, p3, p4, p5, p6, p7 = joints\n",
    "    lines = (\n",
    "        (p1, p2, c(1,2)), (p2, p5, c(2,5)), (p2, p3, c(2,3)), (p0, p3, c(0,3)), (p4, p0, c(4,0)), (p3, p4, c(3,4)), \n",
    "        (p4, p6, c(4,6)), (p0, p5, c(0,5)), (p5, p6, c(5,6)), (p7, p5, c(7,5)), (p6, p7, c(6,7))\n",
    "    )\n",
    "    ax.scatter((p0[0], p1[0]), (p0[1], p1[1]))\n",
    "    for a, b, col in lines:\n",
    "        ax.plot((a[0], b[0]), (a[1], b[1]), color=col)\n",
    "\n",
    "fig = plt.figure(constrained_layout=True)\n",
    "fig.set_size_inches(15, 5)\n",
    "spec2 = gridspec.GridSpec(ncols=6, nrows=2, figure=fig)\n",
    "\n",
    "for i, alpha in enumerate(range(0,360, 30)):\n",
    "    joints = linkage(alpha, x, y, links)\n",
    "    ax = fig.add_subplot(spec2[i//6, i%6])\n",
    "    ax.set_xlim(-70, 60)\n",
    "    ax.set_ylim(-90, 50)\n",
    "    plot(ax, joints)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Assembly"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Parts"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def make_link(length, width=2, height=1):\n",
    "    link = (\n",
    "        cq.Workplane(\"YZ\").rect(length + 4, width + 2)\n",
    "          .pushPoints(((-length/2, 0), (length/2, 0))).circle(1)\n",
    "          .extrude(height).edges(\"|X\").fillet(1.99)\n",
    "    )\n",
    "    link.faces(\">X\").wires(cq.NearestToPointSelector((0, length/2))).tag(\"mate\")\n",
    "    return link\n",
    "\n",
    "parts = {name: make_link(links[name][\"len\"], height=(2 * height if name == \"link_1_2\" else height)) \n",
    "         for name in link_list}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Define Assembly"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def create_leg(x, y):\n",
    "    L = lambda *args: cq.Location(cq.Vector(*args))\n",
    "\n",
    "    leg = MAssembly(cq.Workplane(\"YZ\").polyline([(0,0), (x, 0),(x,y)]), name=\"base\", color=\"Gray\")\n",
    "    for i, name in enumerate(link_list):\n",
    "        leg.add(parts[name], name=name, color=links[name][\"col\"], loc=L(0, 0, i*10 - 50))\n",
    "    return leg\n",
    "\n",
    "leg = create_leg(x, y)\n",
    "d = show(leg, axes=False)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Define Mates"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "leg = create_leg(x, y)\n",
    "\n",
    "for name in link_list:\n",
    "    leg.mate(f\"{name}?mate\", name=name, origin=True)\n",
    "    \n",
    "d = show(leg, render_mates=True, axes=False)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Relocate and assemble"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "leg.relocate()\n",
    " \n",
    "d = show(leg, render_mates=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "alpha = 0\n",
    "joints = linkage(alpha, x, y, links)\n",
    "\n",
    "for name in link_list:\n",
    "    v, a = link_loc(name, joints, links)\n",
    "    abs_loc = cq.Location(cq.Workplane(\"YZ\").plane.rotated((0,0,a)), cq.Vector(*v))  # calculate the absolute location ...\n",
    "    loc = abs_loc * leg.mates[name].mate.loc.inverse                                 # ... and center the mate of the link first\n",
    "    leg.assemble(name, loc)\n",
    "\n",
    "cv = show(leg, render_mates=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Animation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from jupyter_cadquery import AnimationTrack\n",
    "\n",
    "alphas = {name: [] for name in link_list}\n",
    "positions = {name: [] for name in link_list}\n",
    "\n",
    "for alpha in range(0, -375, -15):\n",
    "    for name in link_list:\n",
    "        p, a = link_loc(name, linkage(alpha, x, y, links), links)        \n",
    "        alphas[name].append(a)\n",
    "        positions[name].append(p)\n",
    "\n",
    "time = np.linspace(0, 4, 25)\n",
    "\n",
    "for name in link_list:\n",
    "    cv.add_track(AnimationTrack(f\"/base/{name}\", \"t\",  time, [(p - positions[name][0]).tolist() for p in positions[name]]))\n",
    "    cv.add_track(AnimationTrack(f\"/base/{name}\", \"rz\", time, [a - alphas[name][0] for a in alphas[name]]))\n",
    "\n",
    "cv.animate(2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# close_viewers()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python",
   "pygments_lexer": "ipython3"
  },
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}