{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import cadquery as cq\n",
    "from jupyter_cadquery import (\n",
    "    open_viewer, show,\n",
    "    set_defaults, get_defaults\n",
    ")\n",
    "from cadquery_massembly import Mate, MAssembly, relocate\n",
    "\n",
    "cv = open_viewer(\"Hexapod\", cad_width=800, height=600, theme=\"light\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "set_defaults(axes=False, axes0=True, mate_scale=5, transparent=False)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Hexapod "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![2-hexapod.png](2-hexapod.png)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "horizontal_angle = 25\n",
    "\n",
    "def intervals(count):\n",
    "    r = [ min(180, (90 + i*(360 // count)) % 360) for i in range(count)]\n",
    "    return r \n",
    "\n",
    "def times(end, count):\n",
    "    return np.linspace(0, end, count+1)\n",
    "    \n",
    "def vertical(count, end, offset, reverse):\n",
    "    ints = intervals(count)\n",
    "    heights = [round(35 * np.sin(np.deg2rad(x)) - 15, 1) for x in ints]\n",
    "    heights.append(heights[0])\n",
    "    return times(end, count), heights[offset:] + heights[1:offset+1]\n",
    "\n",
    "def horizontal(end, reverse):\n",
    "    factor = 1 if reverse else -1\n",
    "    return times(end, 4), [0, factor * horizontal_angle, 0, -factor * horizontal_angle, 0]\n",
    "\n",
    "print(\"Leg group 1 (transparent)\")\n",
    "print(\"horizontal movement    \", horizontal(4, True))\n",
    "print(\"vertical heights (left) \", vertical(8, 4, 0, True))\n",
    "print(\"vertical heights (right)\", vertical(8, 4, 0, False))\n",
    "\n",
    "print(\"\\nLeg group 1 (filled)\")\n",
    "print(\"horizontal movement\", horizontal(4, False))\n",
    "print(\"vertical heights (left) \", vertical(8, 4, 4, True))\n",
    "print(\"vertical heights (right)\", vertical(8, 4, 4, False))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Assembly"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Parts"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "thickness = 2\n",
    "height = 40\n",
    "width = 65\n",
    "length = 100\n",
    "diam = 4\n",
    "tol = 0.05"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def create_base():\n",
    "    x1, x2 = 0.63, 0.87\n",
    "    base_holes = {\n",
    "        \"right_front\": (-x1*width, -x1*length), \"right_middle\": (-x2*width, 0), \"right_back\": ( -x1*width, x1*length ),\n",
    "        \"left_front\":  ( x1*width, -x1*length), \"left_middle\":  ( x2*width, 0), \"left_back\":  (  x1*width, x1*length ),\n",
    "    }\n",
    "    stand_holes = {\"front_stand\": (0, -0.75 * length), \"back_stand\": (0, 0.8 * length)}\n",
    "\n",
    "    workplane = cq.Workplane()\n",
    "\n",
    "    base = (workplane\n",
    "        .ellipseArc(width, length, 295, 245, startAtCurrent=False).close()\n",
    "        .pushPoints(list(base_holes.values())).circle(diam / 2 + tol)\n",
    "        .moveTo(*stand_holes[\"back_stand\" ]).rect(width / 2 + 2 * tol, thickness + 2 * tol)\n",
    "        .moveTo(*stand_holes[\"front_stand\"]).rect(width / 2 + 2 * tol, thickness + 2 * tol)\n",
    "        .extrude(thickness)\n",
    "    )\n",
    "    base\n",
    "\n",
    "\n",
    "    base.faces(\"<Z\").tag(\"bottom\")\n",
    "    base.faces(\">Z\").tag(\"top\")\n",
    "\n",
    "    for name, hole in base_holes.items():\n",
    "        base.faces(\"<Z\").wires(cq.NearestToPointSelector(hole)).tag(name)\n",
    "\n",
    "    for name, hole in stand_holes.items():\n",
    "        base.faces(\"<Z\").wires(cq.NearestToPointSelector(hole)).tag(name)\n",
    "\n",
    "    return base\n",
    "\n",
    "base_holes_names = {\"right_back\", \"right_middle\", \"right_front\", \"left_back\", \"left_middle\", \"left_front\"}"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def create_stand():\n",
    "    stand = cq.Workplane().box(height, width / 2 + 10, thickness)\n",
    "    inset = cq.Workplane().box(thickness, width / 2, thickness)\n",
    "    backing = cq.Workplane(\"ZX\").polyline([(10,0), (0,0), (0, 10)]).close().extrude(thickness)\n",
    "\n",
    "    stand = (stand\n",
    "        .union(inset.translate(( (height + thickness) / 2, 0, 0)))\n",
    "        .union(inset.translate((-(height + thickness) / 2, 0, 0)))\n",
    "        .union(backing.translate((-height / 2, -thickness / 2, thickness / 2)))\n",
    "        .union(backing.rotate((0, 0, 0), (0, 1, 0), -90).translate((height / 2, -thickness / 2, thickness / 2)))\n",
    "    )\n",
    "    return stand\n",
    "\n",
    "stand_names = (\"front_stand\", \"back_stand\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def create_upper_leg():\n",
    "    l1, l2 = 50, 80\n",
    "    pts = [( 0,  0), ( 0, height/2), (l1, height/2 - 5), (l2, 0)]\n",
    "    upper_leg_hole = (l2 - 10, 0)\n",
    "\n",
    "    upper_leg = (cq.Workplane()\n",
    "        .polyline(pts).mirrorX()\n",
    "        .pushPoints([upper_leg_hole]).circle(diam/2 + tol).extrude(thickness)\n",
    "        .edges(\"|Z and (not <X)\").fillet(4)\n",
    "    )\n",
    "\n",
    "    axle = (cq.Workplane(\"XZ\", origin=(0, height/2 + thickness + tol, thickness/2))\n",
    "        .circle(diam/2).extrude(2 * (height/2 + thickness + tol))\n",
    "    )\n",
    "\n",
    "    upper_leg = upper_leg.union(axle)\n",
    "\n",
    "    # tag mating points\n",
    "    upper_leg.faces(\">Z\").edges(cq.NearestToPointSelector(upper_leg_hole)).tag(\"top\")\n",
    "    upper_leg.faces(\"<Z\").edges(cq.NearestToPointSelector(upper_leg_hole)).tag(\"bottom\")\n",
    "\n",
    "    return upper_leg\n",
    "\n",
    "def create_lower_leg():\n",
    "    w, l1, l2 = 15, 20, 120\n",
    "    pts = [( 0,  0), ( l1, w), (l2, 0)]\n",
    "    lower_leg_hole = (l1 - 10, 0)\n",
    "\n",
    "    lower_leg = (cq.Workplane()\n",
    "        .polyline(pts).mirrorX()\n",
    "        .pushPoints([lower_leg_hole]).circle(diam/2 + tol)\n",
    "        .extrude(thickness)\n",
    "        .edges(\"|Z\").fillet(5)\n",
    "    )\n",
    "\n",
    "    # tag mating points\n",
    "    lower_leg.faces(\">Z\").edges(cq.NearestToPointSelector(lower_leg_hole)).tag(\"top\"),\n",
    "    lower_leg.faces(\"<Z\").edges(cq.NearestToPointSelector(lower_leg_hole)).tag(\"bottom\")\n",
    "    \n",
    "    return lower_leg\n",
    "\n",
    "leg_angles = {\n",
    "    \"right_back\": -105,  \"right_middle\":-90, \"right_front\":-75, \n",
    "    \"left_back\":   105,  \"left_middle\":  90, \"left_front\":  75,\n",
    "    \n",
    "}\n",
    "leg_names = list(leg_angles.keys())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "base = create_base()\n",
    "stand = create_stand()\n",
    "upper_leg = create_upper_leg()\n",
    "lower_leg = create_lower_leg()\n",
    "\n",
    "cv = show(\n",
    "    base, \n",
    "    stand.translate((100, -60, thickness/2)), \n",
    "    upper_leg.translate((80,0,0)),\n",
    "    lower_leg.translate((75,60,0)),\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Define Assembly"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def create_hexapod():\n",
    "    # Some shortcuts\n",
    "    L = lambda *args: cq.Location(cq.Vector(*args))\n",
    "\n",
    "    # Leg assembly\n",
    "    leg = (MAssembly(upper_leg, name=\"upper\", color=\"orange\")\n",
    "        .add(lower_leg, name=\"lower\", color=\"orange\", loc=L(80,0,0))\n",
    "    )\n",
    "    # Hexapod assembly\n",
    "    hexapod = (MAssembly(base, name=\"bottom\", color=\"silver\")\n",
    "        .add(base, name=\"top\", color=\"gainsboro\", loc=L(0, -2*length, 0))\n",
    "        .add(stand, name=\"front_stand\", color=\"SkyBlue\", loc=L(120, -255, 0))\n",
    "        .add(stand, name=\"back_stand\", color=\"SkyBlue\", loc=L(180, -255, 0))\n",
    "    )\n",
    "    \n",
    "    for i, name in enumerate(leg_names):\n",
    "        hexapod.add(leg, name=name, loc=L(100, -55*(i-1.4), 0))\n",
    "\n",
    "    return hexapod"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Define Mates "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from collections import OrderedDict as odict\n",
    "\n",
    "hexapod = create_hexapod()\n",
    "\n",
    "hexapod.mate(\"bottom?top\", name=\"bottom\", origin=True)\n",
    "hexapod.mate(\"top?bottom\", name=\"top\", origin=True, transforms=odict(rx=180, tz=-(height + 2 * tol)))\n",
    "\n",
    "for name in stand_names:\n",
    "    hexapod.mate(f\"bottom?{name}\", name=f\"{name}_bottom\", transforms=odict(rz=180 if \"f\" in name else 0))\n",
    "    hexapod.mate(f\"{name}@faces@<X\", name=name, origin=True, transforms=odict(rx=180))\n",
    "\n",
    "for name in base_holes_names:\n",
    "    hexapod.mate(f\"bottom?{name}\", name=f\"{name}_hole\", transforms=odict(rz=leg_angles[name]))\n",
    "\n",
    "for name in leg_names:\n",
    "    lower, upper, angle = (\"top\", \"bottom\", -75) if \"left\" in name else (\"bottom\", \"top\", -75)\n",
    "    hexapod.mate(f\"{name}?{upper}\", name=f\"leg_{name}_hole\", transforms=odict(rz=angle))\n",
    "    hexapod.mate(f\"{name}@faces@<Y\", name=f\"leg_{name}_hinge\", origin=True, transforms=odict(rx=180))\n",
    "    hexapod.mate(f\"{name}/lower?{lower}\", name=f\"leg_{name}_lower_hole\", origin=True)\n",
    "\n",
    "cv = show(hexapod, render_mates=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Relocate and assemble"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "hexapod.relocate()\n",
    "cv = show(hexapod)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for leg in leg_names:\n",
    "    hexapod.assemble(f\"leg_{leg}_lower_hole\", f\"leg_{leg}_hole\")\n",
    "    hexapod.assemble(f\"leg_{leg}_hinge\", f\"{leg}_hole\")\n",
    "\n",
    "hexapod.assemble(\"top\", \"bottom\")\n",
    "\n",
    "for stand_name in stand_names:\n",
    "    hexapod.assemble(f\"{stand_name}\", f\"{stand_name}_bottom\")\n",
    "\n",
    "\n",
    "cv = show(hexapod, render_mates=False, grid=[True, False, False], axes=False, render_normals=False)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Animation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from jupyter_cadquery import AnimationTrack\n",
    "\n",
    "leg_group = (\"left_front\", \"right_middle\", \"left_back\")\n",
    "\n",
    "for name in leg_names:\n",
    "    # move upper leg\n",
    "    cv.add_track(AnimationTrack(\n",
    "        f\"/bottom/{name}\", \n",
    "        \"rz\", *horizontal(4, \"middle\" in name)\n",
    "    ))\n",
    "\n",
    "    # move lower leg\n",
    "    cv.add_track(AnimationTrack(\n",
    "        f\"/bottom/{name}/lower\", \n",
    "        \"rz\", *vertical(8, 4, 0 if name in leg_group else 4, \"left\" in name)\n",
    "    ))\n",
    "    \n",
    "    # lift hexapod to run on grid\n",
    "    # animation.add_track(f\"/bottom\", \"tz\", [0, 4], [-118]*2)\n",
    "    \n",
    "cv.animate(speed=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
}