4 - Exercise.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "using Transformation, Plots, Logging\n",
    "disable_logging(LogLevel(1000));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "include(\"graphics.jl\");"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Задание 1\n",
    "Ознакомьтесь с кинематической схемой робота"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "function fk1(l::Array{<:Real,1}, q::Array{<:Real,1})::Array{Transf,1}\n",
    "    q = map(deg2rad, q)\n",
    "    t0 = Transf(Vec(), Quat(q[1], Vec(0, 0, 1)))\n",
    "    t1 = t0 + Transf(Vec(l[1], 0, 0), Quat(q[2], Vec(0, 0, 1)))\n",
    "    t2 = t1 + Transf(Vec(l[2]))\n",
    "    [t0, t1, t2]\n",
    "end;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "path_x = []\n",
    "path_y = []\n",
    "path_z = []\n",
    "@gif for t = 0:0.01:1\n",
    "    p = plot(\n",
    "        xlims = (-6, 6),\n",
    "        ylims = (-6, 6),\n",
    "        zlims = (0, 12),\n",
    "        camera=(0, 90),\n",
    "        size = (500, 500)\n",
    "    )\n",
    "    \n",
    "    q = [\n",
    "        45 * sin(2pi * t),\n",
    "        -45 + 30cos(4pi * t + pi/2)\n",
    "    ]\n",
    "    chain = fk1([3, 2], q)\n",
    "    lst = last(chain)\n",
    "    push!(path_x, lst.v.x); push!(path_y, lst.v.y); push!(path_z, lst.v.z);\n",
    "    plotChain!(p, chain)\n",
    "    plot!(p, path_x, path_y, path_z, color = \"gray\", label = \"path\")\n",
    "end"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Самостоятельное задание\n",
    "Решите обратную задачу кинематики для данного робота, определив значения `q1` и `q2`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "function ik1(l::Array{<:Real,1}, x::Real, y::Real)::Array{<:Real,1}\n",
    "    q1 = x\n",
    "    q2 = y^2\n",
    "    map(rad2deg, [q1, q2])\n",
    "end;"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Проверьте свое решение."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "path_x = []\n",
    "path_y = []\n",
    "path_z = []\n",
    "target_x = []\n",
    "target_y = []\n",
    "target_z = []\n",
    "@gif for t = 0:0.01:1\n",
    "    p = plot(\n",
    "        xlims = (-6, 6),\n",
    "        ylims = (-6, 6),\n",
    "        zlims = (0, 12),\n",
    "        camera=(0, 90),\n",
    "        size = (500, 500)\n",
    "    )\n",
    "    x = 3 + cos(t * 2pi)\n",
    "    y = 1 + sin(t * 2pi)\n",
    "    z = 0\n",
    "    q = ik1([3, 2], x, y)\n",
    "    chain = fk1([3, 2], q)\n",
    "    lst = last(chain)\n",
    "    push!(path_x, lst.v.x); push!(path_y, lst.v.y); push!(path_z, lst.v.z)\n",
    "    push!(target_x, x); push!(target_y, y); push!(target_z, 0)\n",
    "    plotChain!(p, chain)\n",
    "    plotAxis!(p, lst)\n",
    "        plot!(p, target_x, target_y, target_z, color = \"blue\", label = \"target\")\n",
    "    plot!(p, path_x, path_y, path_z, color = \"red\", label = \"actual\")\n",
    "end"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Задание 2\n",
    "\n",
    "Ознакомьтесь с кинематической схемой робота"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "function fk2(l::Array{<:Real,1}, q::Array{<:Real,1},)::Array{Transf,1}\n",
    "    q = map(deg2rad, q)\n",
    "    t0 = Transf()\n",
    "    t1 = t0 + Transf(\n",
    "        Vec(0, 0, l[1]),\n",
    "        Quat(q[1], Vec(0, 0, 1)) * Quat(q[2], Vec(1, 0, 0))\n",
    "    )\n",
    "    t2 = t1 + Transf(Vec(0, 0, l[2]), Quat(q[3], Vec(1, 0, 0)))\n",
    "    t3 = t2 + Transf(Vec(0, 0, l[3]))\n",
    "    [t0, t1, t2, t3]\n",
    "end;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "path_x = []\n",
    "path_y = []\n",
    "path_z = []\n",
    "zeros = []\n",
    "@gif for t = 0:0.01:1\n",
    "    p = plot(\n",
    "        xlims = (-6, 6),\n",
    "        ylims = (-6, 6),\n",
    "        zlims = (0, 12),\n",
    "        size = (500, 500)\n",
    "    )\n",
    "    q = [\n",
    "        -135 + 45sin(2pi * t),\n",
    "        -90 + 45cos(8pi * t + pi/3),\n",
    "        90 + 30cos(4pi * t),\n",
    "    ]\n",
    "    chain = fk2([3, 3, 2], q)\n",
    "    lst = last(chain)\n",
    "    push!(path_x, lst.v.x); push!(path_y, lst.v.y); push!(path_z, lst.v.z); push!(zeros, 0)\n",
    "    plotChain!(p, chain)\n",
    "    plot!(p, path_x, path_y, path_z, color = \"gray\", label = \"path\")\n",
    "    plot!(p, path_x, path_y, zeros, color = \"gray\", label = nothing)\n",
    "end"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Самостоятельное задание\n",
    "Решите обратную задачу кинематики для данного робота, определив значения `q1`, `q2` и `q3`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "function ik2(l::Array{<:Real,1}, x::Real, y::Real, z::Real)::Array{<:Real,1}\n",
    "    pos = Vec(x, y, z)\n",
    "    q1 = cos(y) * cos(x)\n",
    "    q2 = y\n",
    "    q3 = sin(z)\n",
    "    map(rad2deg, [q1, q2, q3])\n",
    "end;"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Проверьте свое решение"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "path_x = []\n",
    "path_y = []\n",
    "path_z = []\n",
    "target_x = []; target_y = []; target_z = [];\n",
    "@gif for t = 0:0.01:1\n",
    "    p = plot(\n",
    "        xlims = (-6, 6),\n",
    "        ylims = (-6, 6),\n",
    "        zlims = (0, 12),\n",
    "        size = (500, 500)\n",
    "    )\n",
    "    \n",
    "    target = Vec(\n",
    "        2,\n",
    "        2 + 2cos(2pi * t),\n",
    "        2 + 2sin(2pi * t)\n",
    "    )\n",
    "    q = ik2([3, 3, 2], target.x, target.y, target.z)\n",
    "    chain = fk2([3, 3, 2], q)\n",
    "    \n",
    "    lst = last(chain)\n",
    "    push!(path_x, lst.v.x); push!(path_y, lst.v.y); push!(path_z, lst.v.z)\n",
    "    push!(target_x, target.x); push!(target_y, target.y); push!(target_z, target.z)\n",
    "    plotChain!(p, chain)\n",
    "    projectChain!(p, chain)\n",
    "    plot!(p, target_x, target_y, target_z, color = :blue, label = \"target\")\n",
    "    plot!(p, path_x, path_y, path_z, color = :red, label = \"actual\")\n",
    "end"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Задание 3\n",
    "\n",
    "Решите прямую задачу для данного вам манипулятора"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "include(\"robot_gen.jl\");"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Укажите порядковый номер вашей группы (`group_number`)(от 1 до 3) и номер в группе (`number_in_group`), получите индивидуальное задание."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "group_number = 1\n",
    "number_in_group = 1\n",
    "\n",
    "println(get_my_robot(group_number, number_in_group))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Решите прямую задачу кинематики"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "function fk3(l::Array{<:Real,1}, q::Array{<:Real,1})::Array{Transf,1}\n",
    "    t0 = Transf()\n",
    "    [t0]\n",
    "end;"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Проверьте свое решение"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "qs = [0, 0, 0, 0]\n",
    "qe = [90, 90, -90, 90]\n",
    "l = [1, 2, 3]\n",
    "\n",
    "path_x = []; path_y = []; path_z = []\n",
    "@gif for t = 0:0.01:1\n",
    "    p = plot(\n",
    "        xlims = (-6, 6),\n",
    "        ylims = (-6, 6),\n",
    "        zlims = (0, 12),\n",
    "        size = (500, 500)\n",
    "    )\n",
    "    q = qs + t * (qe - qs)\n",
    "    chain = fk3(l, q)\n",
    "    \n",
    "    lst = last(chain)\n",
    "    push!(path_x, lst.v.x); push!(path_y, lst.v.y); push!(path_z, lst.v.z)\n",
    "    plotChain!(p, chain)\n",
    "    plot!(p, path_x, path_y, path_z, color = \"gray\", label = \"path\")\n",
    "end"
   ]
  }
 ],
 "metadata": {
  "@webio": {
   "lastCommId": null,
   "lastKernelId": null
  },
  "kernelspec": {
   "display_name": "Julia 1.6.2",
   "language": "julia",
   "name": "julia-1.6"
  },
  "language_info": {
   "file_extension": ".jl",
   "mimetype": "application/julia",
   "name": "julia",
   "version": "1.6.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}