{ "cells": [ { "cell_type": "markdown", "id": "379cb836", "metadata": {}, "source": [ "## Example: Kinematics" ] }, { "cell_type": "code", "execution_count": 1, "id": "dff598f1", "metadata": { "colab": { "base_uri": "https://localhost:8080/" }, "id": "dff598f1", "outputId": "40683e10-da68-4d71-9380-c615c58546bd" }, "outputs": [ { "data": { "text/plain": [ "'\\n%pip install nullspace\\n'" ] }, "execution_count": 1, "metadata": {}, "output_type": "execute_result" } ], "source": [ "'''\n", "%pip install nullspace\n", "'''" ] }, { "cell_type": "code", "execution_count": 2, "id": "faee7a2e", "metadata": { "id": "faee7a2e" }, "outputs": [], "source": [ "from sympy import sin, cos, eye\n", "from sympy import symbols, simplify, nsimplify\n", "from sympy import Array, Matrix, ZeroMatrix, BlockMatrix\n", "\n", "import sympy as sp\n", "import numpy as np\n", "\n", "from nullspace.rbda import Rx, Ry, Rz\n", "from nullspace.se3 import hat, Ex, Ey, Ez, SXForm, getE, getr, perfect_Ez_pi\n", "from nullspace import ensure_positive_Z, autodiff_svd, svd_regulated" ] }, { "cell_type": "code", "execution_count": 3, "id": "76a687c4", "metadata": {}, "outputs": [], "source": [ "from nullspace.models.structure import q_sym\n", "from nullspace.models.structure import robotBuilder\n", "\n", "o = robotBuilder('structure.yaml')\n", "abad0 = o[0][0]\n", "abad1 = o[1][0]\n", "abad2 = o[2][0]\n", "abad3 = o[3][0]\n", "\n", "hip0 = o[0][1]\n", "hip1 = o[1][1]\n", "hip2 = o[2][1]\n", "hip3 = o[3][1]\n", "\n", "knee0 = o[0][2]\n", "knee1 = o[1][2]\n", "knee2 = o[2][2]\n", "knee3 = o[3][2]\n", "\n", "foot0 = o[0][3]\n", "foot1 = o[1][3]\n", "foot2 = o[2][3]\n", "foot3 = o[3][3]" ] }, { "cell_type": "code", "execution_count": 4, "id": "7b452ee6", "metadata": { "id": "7b452ee6" }, "outputs": [], "source": [ "# Control variables\n", "'''\n", " Top view of distance vectors.\n", " v3\n", " 1<-------0\n", " |\\ /|\n", " | \\ v2/ |\n", " | \\ / |\n", " | \\/ |\n", " v1| /\\ |v4\n", " | / \\ |\n", " | / v0\\ |\n", " vv vv\n", " 3<-------2\n", " v5\n", "''' \n", "\n", "v0 = foot2 - foot1\n", "v1 = foot3 - foot1\n", "v2 = foot3 - foot0\n", "v3 = foot1 - foot0\n", "v4 = foot2 - foot0\n", "v5 = foot3 - foot2\n", "\n", "# 6 distances between each foots\n", "s0 = v0.transpose() * v0\n", "s1 = v1.transpose() * v1\n", "s2 = v2.transpose() * v2\n", "s3 = v3.transpose() * v3\n", "s4 = v4.transpose() * v4\n", "s5 = v5.transpose() * v5\n", "\n", "dist = Matrix([s0, s1, s2, s3, s4, s5])\n", "n = v5.cross(v1)\n", "cen = (foot0 + foot1 + foot2 + foot3) / 4\n", "A = Matrix([foot0 - cen, foot1 - cen, foot2 - cen, foot3 - cen]).reshape(4,3).T\n", "\n", "var = Matrix(q_sym)" ] }, { "cell_type": "code", "execution_count": 5, "id": "6c937908", "metadata": { "id": "6c937908" }, "outputs": [], "source": [ "# Jacobian of control variables\n", "\n", "jc = dist.jacobian(var)\n", "\n", "# For support_normal_option = 'cross'\n", "j_ori = Matrix([n, v5]).jacobian(var)\n", "# This format matches the frist two columns of normalized orthogonal basis of orientation.\n", "# The 3rd/last column isn't needed here since there are only 6 independent vars and other\n", "# 3 could be derived.\n", "\n", "# For support_normal_option = 'svd'\n", "j_ori_tail3 = v5.jacobian(var)\n", "# j_ori_head3 will be achieved by autodiff_svd directly at specific q_star,\n", "# there isn't analytical jacobian expression for derivative of normal vector if\n", "# normal vector is calculated by best fitting from SVD.\n", "\n", "j_pos = cen.jacobian(var)\n", "\n", "#jc = jc.subs(links_params)\n", "#j_ori = j_ori.subs(links_params)\n", "#j_pos = j_pos.subs(links_params)" ] }, { "cell_type": "code", "execution_count": 6, "id": "6c3c7cab", "metadata": { "id": "6c3c7cab" }, "outputs": [], "source": [ "# FourLegRobot and ThreeLinkArm\n", "\n", "\n", "%matplotlib notebook\n", "%matplotlib notebook \n", "# DON'T DELETE, This isn't redundant in my knowledge of 2020\n", "\n", "\n", "from math import cos, sin, pi\n", "import numpy as np\n", "import matplotlib.pyplot as plt\n", "import mpl_toolkits.mplot3d.art3d as art3d\n", "from mpl_toolkits.mplot3d.art3d import Poly3DCollection\n", "from matplotlib.patches import Circle, PathPatch, Rectangle, Polygon\n", "import time\n", "\n", "\n", "\n", "\n", "R_floor = np.array([[ 0.8660254, 0. , -0.5 ],\n", " [ 0. , 1. , 0. ],\n", " [ 0.5 , 0. , 0.8660254]])\n", "# R_floor = Ry(-pi/6) @ Rx(pi/6)\n", "Pos_floor = np.array([0.2, 0.1, 0.3])\n", "\n", "\n", "class ThreeLinkArm:\n", " def __init__(self, joint_angles, num, A, H, K, F):\n", " '''\n", " joint_angles:\n", " The 3d vector of abad, hip and knee joint value.\n", " \n", " num:\n", " The number of iteration. This number is used to\n", " control the alpha of Arm plot.\n", " \n", " A, H, K and F: sympy objs\n", " Each of them could be thought as a mathematical\n", " function which takes 3 joints value and return\n", " abad, hip, knee and foot positions.\n", " '''\n", " self.body_ax = None\n", " # ax is dependent injection\n", " self.num = num\n", " self.abad = np.array([i for i in A], dtype=float)\n", " self.H = H\n", " self.K = K\n", " self.F = F\n", "\n", "\n", " def forward_kinematics(self, name):\n", " if name == 0:\n", " d = {q_sym[0]: self.joint_angles[0],\n", " q_sym[1]: self.joint_angles[1],\n", " q_sym[2]: self.joint_angles[2]}\n", " elif name == 1:\n", " d = {q_sym[3]: self.joint_angles[0],\n", " q_sym[4]: self.joint_angles[1],\n", " q_sym[5]: self.joint_angles[2]}\n", " elif name == 2:\n", " d = {q_sym[6]: self.joint_angles[0],\n", " q_sym[7]: self.joint_angles[1],\n", " q_sym[8]: self.joint_angles[2]}\n", " else:\n", " d = {q_sym[9]: self.joint_angles[0],\n", " q_sym[10]: self.joint_angles[1],\n", " q_sym[11]: self.joint_angles[2]}\n", " \n", " self.hip = np.array([i for i in self.H.subs(d)], dtype=float)\n", " self.knee = np.array([i for i in self.K.subs(d)], dtype=float)\n", " self.foot = np.array([i for i in self.F.subs(d)], dtype=float)\n", "\n", "\n", " def plot(self, i, abad_color, hip_color, knee_color):\n", "\n", " # plot one leg. index 0, 1 and 2 are meaning x, y, z composite here\n", " # plot abad - hip arm\n", " self.body_ax.plot([self.abad[0], self.hip[0]],\n", " [self.abad[1], self.hip[1]],\n", " [self.abad[2], self.hip[2]],\n", " color = abad_color)\n", " \n", " # plot hip - knee\n", " self.body_ax.plot([self.hip[0], self.knee[0]],\n", " [self.hip[1], self.knee[1]],\n", " [self.hip[2], self.knee[2]],\n", " color = hip_color)\n", " \n", " # plot knee - foot\n", " self.body_ax.plot([self.knee[0], self.foot[0]],\n", " [self.knee[1], self.foot[1]],\n", " [self.knee[2], self.foot[2]],\n", " color = knee_color)\n", " \n", " if i == num - 1:\n", " self.body_ax.plot(self.foot[0],self.foot[1], self.foot[2],'b')\n", " \n", "\n", "\n", "class FourLegRobot:\n", " def __init__(self, zeroth, first, second, third, num, support_normal_option):\n", " \n", " self.num = num\n", " self.it = None\n", " # Ori\n", " # Support and Body\n", " self.support_normal_option = support_normal_option\n", " self.EdgeY_SupportOnBody_Target = None\n", " self.EdgeY_SupportOnBody_Current = None\n", " self.Normal_SupportOnBody_Target = None\n", " self.Normal_SupportOnBody_Current = None\n", " \n", " self.Ori_SupportOnBody_Target = None\n", " self.Ori_SupportOnBody_Current = None\n", " self.Ori_SupportOnBody_Error = np.array([0] * 6, dtype = float)\n", " self.Ori_BodyOnSupport_Target = None\n", " self.Ori_BodyOnSupport_Current = None \n", " self.Ori_SupportOnHorizon_Current = None # R0, Estimation of Floor Ori\n", " \n", " # Body and Horizon\n", " # This is INPUT target of ori which in Horioin Coordinate\n", " self.Ori_BodyOnHorizon_Target = None\n", " self.Ori_BodyOnHorizon_Current = None\n", " self.IMU_BodyOnHorizon_Sim = None # A alias of self.Ori_BodyOnHorizon_Current\n", "\n", " # Pos\n", " # This is INPUT target of pos which is calibrated to Horizontal Support Coordinate\n", " # to make it match human visual feelings and dynamics.\n", " # This is HorizontalSupport not Horizon since we don't no/don't need to know\n", " # where the robot is. It might be on Everest or someone's home.\n", " self.Pos_BodyOnHorizontalSupport_Target = None \n", "\n", " self.Pos_BodyOnSupport_Target = None\n", " self.Pos_SupportOnBody_Target = None\n", " self.Pos_SupportCentroidOnBody_Current = None\n", " self.Pos_SupportOnBody_Error = None\n", " \n", " self.arms = [None, None, None, None]\n", " self.arms[0] = zeroth\n", " self.arms[1] = first\n", " self.arms[2] = second\n", " self.arms[3] = third\n", " self.body_ax = plt.subplot(3, 1, 1, projection='3d')\n", " self.support_ax = plt.subplot(3, 1, 2, projection='3d')\n", " self.horizon_ax = plt.subplot(3, 1, 3, projection='3d')\n", "\n", " start_point = self.arms[2].abad[:2]\n", " self.body_length = self.arms[1].abad[0] * 2 \n", " self.body_width = self.arms[1].abad[1] * 2\n", " rec = Rectangle(start_point,\n", " self.body_length,\n", " self.body_width,\n", " angle=0.0,\n", " color=(0.1,0.1,0.1),\n", " alpha = 0.2)\n", " # r: Base body which is simplified to a rectangle\n", " \n", " self.body_ax.add_patch(rec)\n", " art3d.pathpatch_2d_to_3d(rec, z=0, zdir=\"z\")\n", " \n", " self.body_ax.set_xlim(-0.3, 0.3)\n", " self.body_ax.set_ylim(-0.3, 0.3)\n", " self.body_ax.set_zlim(-0.4, 0.2)\n", " \n", " self.support_ax.set_xlim(-0.3, 0.3)\n", " self.support_ax.set_ylim(-0.3, 0.3)\n", " self.support_ax.set_zlim(-0.2, 0.4) \n", " \n", " self.horizon_ax.set_xlim(-0.3, 0.3)\n", " self.horizon_ax.set_ylim(-0.3, 0.3)\n", " self.horizon_ax.set_zlim(0, 0.6)\n", " \n", " \n", " plt.gca().set_aspect(\"auto\")\n", " self.arms[0].body_ax = self.body_ax\n", " self.arms[1].body_ax = self.body_ax\n", " self.arms[2].body_ax = self.body_ax\n", " self.arms[3].body_ax = self.body_ax\n", " \n", " \n", " \n", " def set_Ori_BodyOnSupport_Target_byRotation(self, R):\n", " '''\n", " Set Ori by Rotation matrix of Body relative to support area.\n", " '''\n", " self.Ori_BodyOnSupport_Target = R\n", " self.Ori_SupportOnBody_Target = self.Ori_BodyOnSupport_Target.T\n", " self.Normal_SupportOnBody_Target = self.Ori_SupportOnBody_Target[:, 2]\n", " self.EdgeY_SupportOnBody_Target = self.Ori_SupportOnBody_Target[:, 1] \n", " \n", " def set_Ori_BodyOnSupport_byYZ(self, yz):\n", " col1 = yz[:3]\n", " col2 = yz[3:]\n", " col1 = col1 / np.linalg.norm(col1)\n", " col2 = col2 / np.linalg.norm(col2)\n", " col0 = np.cross(col1, col2)\n", " R = np.column_stack((col0, col1, col2))\n", " self.set_Ori_BodyOnSupport_Target_byRotation(R)\n", "\n", " def set_Ori_BodyOnSupport_Target_byRPY(self, roll, pitch, yaw):\n", " R = Rz(yaw) @ Ry(pitch) @ Rx(roll)\n", " self.set_Ori_BodyOnSupport_Target_byRotation(R)\n", " \n", " def set_Ori_BodyOnHorizon_Target_byRPY(self, roll, pitch, yaw):\n", " self.Ori_BodyOnHorizon_Target = Rz(yaw) @ Ry(pitch) @ Rx(roll)\n", " \n", " def set_Pos_SupportOnBody_Target(self, Pos_BodyOnSupport_Target):\n", " self.Pos_SupportOnBody_Target = -(self.Ori_SupportOnBody_Target\n", " @ Pos_BodyOnSupport_Target)\n", " \n", " def set_Ori_SupportOnBody_Current(self, edgeY, normal):\n", " x = np.cross(edgeY, normal)\n", " self.Ori_SupportOnBody_Current = np.column_stack((x, edgeY, normal))\n", " \n", " def get_edgeY(self):\n", " '''\n", " Top View \n", " \n", " 1 --- 0\n", " | |\n", " | |\n", " | |\n", " 3 --- 2\n", " \n", " Get the edge vector along Y-ish direction inside the best-fit plane.\n", " \n", " The edge vector start point is the projection point of middle point\n", " between foot 0 and foot 2 on best-fit plane.\n", " The edge vector end point is the projection point of middle point\n", " between foot 1 and foot 3 on best-fit plane.\n", " \n", " Returns\n", " -------\n", " vec : np.array\n", " edgeY-ish vector\n", " \n", " '''\n", " if self.support_normal_option == 'svd':\n", " middle_point_bt13 = (self.arms[1].foot + self.arms[3].foot) / 2\n", " mp13_projection = self.get_point_projection_on_plane(self.Pos_SupportCentroidOnBody_Current,\n", " self.Normal_SupportOnBody_Current,\n", " middle_point_bt13)\n", "\n", " middle_point_bt02 = (self.arms[0].foot + self.arms[2].foot) / 2\n", " mp02_projection = self.get_point_projection_on_plane(self.Pos_SupportCentroidOnBody_Current,\n", " self.Normal_SupportOnBody_Current,\n", " middle_point_bt02) \n", " vec = mp13_projection - mp02_projection\n", " else:\n", " vec = self.arms[3].foot - self.arms[2].foot\n", " return vec\n", " \n", " def get_point_projection_on_plane(self, c, n, p):\n", " '''\n", " Calculate the point coordinate on certain plane.\n", " \n", " Parameters\n", " ----------\n", " c : numpy array\n", " Plane is discribed by a point and normal vector.\n", " c is the point of this discription.\n", " n : numpy array\n", " Plane is discribed by a point and normal vector.\n", " n is the normal vector of this discription.\n", " p : numpy array\n", " The target point in space that need to be projected\n", " on plane.\n", " \n", " Returns\n", " -------\n", " res : numpy array\n", " Point coordinate on plane.\n", " \n", " '''\n", " \n", " t = sum(n * c) - sum(n * p) / sum(n * n)\n", " # * is element-wise here. \n", " res = p + t * n\n", " return res\n", "\n", " \n", " def update_joints(self, q_values):\n", " self.arms[0].joint_angles = q[0:3]\n", " self.arms[1].joint_angles = q[3:6]\n", " self.arms[2].joint_angles = q[6:9]\n", " self.arms[3].joint_angles = q[9:12]\n", " \n", " self.arms[0].forward_kinematics(0)\n", " self.arms[1].forward_kinematics(1)\n", " self.arms[2].forward_kinematics(2)\n", " self.arms[3].forward_kinematics(3)\n", " \n", " \n", " def prepare_plot(self):\n", "\n", " progress = self.it / self.num\n", " \n", " self.base_color = (0.8 - 0.7 * progress,\n", " 0.8 - 0.7 * progress,\n", " 0.8 - 0.7 * progress,\n", " 0.7)\n", " \n", " self.abad_color = (0.8 - 0.7 * progress,\n", " 0.8 - 0.7 * progress,\n", " 0.8 - 0.7 * progress,\n", " 0.7)\n", "\n", " self.hip_color = (0.7 - 0.7 * progress,\n", " 0.8 - 0.7 * progress,\n", " 0.9 - 0.7 * progress,\n", " 0.7)\n", "\n", " self.knee_color = (0.9 - 0.5 * progress,\n", " 0.8 - 0.5 * progress,\n", " 0.7 - 0.5 * progress,\n", " 0.7)\n", "\n", " self.alpha = (self.it + 1) / self.num\n", " self.support_color = (0.745, 0.458, 0.941) # Purple\n", " def plot_on_body(self):\n", "\n", " # Plot each arms\n", " self.arms[0].plot(self.it, self.abad_color, self.hip_color, self.knee_color)\n", " self.arms[1].plot(self.it, self.abad_color, self.hip_color, self.knee_color)\n", " self.arms[2].plot(self.it, self.abad_color, self.hip_color, self.knee_color)\n", " self.arms[3].plot(self.it, self.abad_color, self.hip_color, self.knee_color)\n", " \n", " # Plot quadrilateral supporting area in purple color(\"0.745, 0.458, 0.941\")\n", "\n", "\n", " self.body_ax.plot([self.arms[1].foot[0], self.arms[0].foot[0]],\n", " [self.arms[1].foot[1], self.arms[0].foot[1]],\n", " [self.arms[1].foot[2], self.arms[0].foot[2]],\n", " color=self.support_color,\n", " alpha=self.alpha)\n", " \n", " self.body_ax.plot([self.arms[3].foot[0], self.arms[1].foot[0]],\n", " [self.arms[3].foot[1], self.arms[1].foot[1]],\n", " [self.arms[3].foot[2], self.arms[1].foot[2]],\n", " color=self.support_color,\n", " alpha=self.alpha)\n", " \n", " self.body_ax.plot([self.arms[3].foot[0], self.arms[2].foot[0]],\n", " [self.arms[3].foot[1], self.arms[2].foot[1]],\n", " [self.arms[3].foot[2], self.arms[2].foot[2]],\n", " color=self.support_color,\n", " alpha=self.alpha)\n", "\n", " self.body_ax.plot([self.arms[0].foot[0], self.arms[2].foot[0]],\n", " [self.arms[0].foot[1], self.arms[2].foot[1]],\n", " [self.arms[0].foot[2], self.arms[2].foot[2]],\n", " color=self.support_color,\n", " alpha=self.alpha)\n", " \n", " \n", " def plot_on_support_each_arm(self, arm):\n", " '''\n", " Calculate each joints location of one leg under horizon coordinate.\n", " There are 4 joints and 3 links.\n", " The location of abad joint is identical with node location on base body.\n", " '''\n", " pos = self.Pos_SupportCentroidOnBody_Current\n", " R = self.Ori_SupportOnBody_Current\n", " \n", " abad_on_support = R.T @ (arm.abad - pos)\n", " hip_on_support = R.T @ (arm.hip - pos)\n", " knee_on_support = R.T @ (arm.knee - pos)\n", " foot_on_support = R.T @ (arm.foot - pos)\n", "\n", "\n", " self.support_ax.plot([abad_on_support[0], hip_on_support[0]],\n", " [abad_on_support[1], hip_on_support[1]],\n", " [abad_on_support[2], hip_on_support[2]],\n", " color=self.abad_color)\n", " \n", " self.support_ax.plot([hip_on_support[0], knee_on_support[0]],\n", " [hip_on_support[1], knee_on_support[1]],\n", " [hip_on_support[2], knee_on_support[2]],\n", " color=self.hip_color)\n", " \n", " self.support_ax.plot([knee_on_support[0], foot_on_support[0]],\n", " [knee_on_support[1], foot_on_support[1]],\n", " [knee_on_support[2], foot_on_support[2]],\n", " color=self.knee_color)\n", "\n", "\n", " \n", " def plot_on_support(self):\n", " '''\n", " Plot on support area coordinate with Centroid as origin.\n", " '''\n", " pos = self.Pos_SupportCentroidOnBody_Current\n", " R = self.Ori_SupportOnBody_Current\n", " \n", " # Calculate base body rectangle, 4 nodes, location on support coordinate\n", " s0 = R.T @ (self.arms[0].abad - pos)\n", " s1 = R.T @ (self.arms[1].abad - pos)\n", " s2 = R.T @ (self.arms[2].abad - pos)\n", " s3 = R.T @ (self.arms[3].abad - pos)\n", " \n", " # Plot base body on support_ax\n", " self.support_ax.plot([s0[0], s1[0]],\n", " [s0[1], s1[1]],\n", " [s0[2], s1[2]],\n", " color=self.base_color)\n", " \n", " self.support_ax.plot([s0[0], s2[0]],\n", " [s0[1], s2[1]],\n", " [s0[2], s2[2]],\n", " color=self.base_color)\n", " \n", " self.support_ax.plot([s3[0], s1[0]],\n", " [s3[1], s1[1]],\n", " [s3[2], s1[2]],\n", " color=self.base_color)\n", " \n", " self.support_ax.plot([s3[0], s2[0]],\n", " [s3[1], s2[1]],\n", " [s3[2], s2[2]],\n", " color=self.base_color)\n", " \n", " # Plot 4 legs\n", " for i in range(4):\n", " self.plot_on_support_each_arm(self.arms[i])\n", "\n", " \n", " def plot_on_horizon_each_arm(self, arm):\n", " '''\n", " Calculate each joints location of one leg under horizon coordinate.\n", " There are 4 joints and 3 links.\n", " The location of abad joint is identical with node location on base body.\n", " '''\n", " pos = self.Pos_SupportCentroidOnBody_Current \n", " R = self.Ori_SupportOnBody_Current\n", " \n", "\n", " abad_on_horizon = R_floor @ (R.T @ (arm.abad - pos) + Pos_floor)\n", " hip_on_horizon = R_floor @ (R.T @ (arm.hip - pos) + Pos_floor)\n", " knee_on_horizon = R_floor @ (R.T @ (arm.knee - pos) + Pos_floor)\n", " foot_on_horizon = R_floor @ (R.T @ (arm.foot - pos) + Pos_floor)\n", "\n", "\n", " self.horizon_ax.plot([abad_on_horizon[0], hip_on_horizon[0]],\n", " [abad_on_horizon[1], hip_on_horizon[1]],\n", " [abad_on_horizon[2], hip_on_horizon[2]],\n", " color=self.abad_color)\n", " \n", " self.horizon_ax.plot([hip_on_horizon[0], knee_on_horizon[0]],\n", " [hip_on_horizon[1], knee_on_horizon[1]],\n", " [hip_on_horizon[2], knee_on_horizon[2]],\n", " color=self.hip_color)\n", " \n", " self.horizon_ax.plot([knee_on_horizon[0], foot_on_horizon[0]],\n", " [knee_on_horizon[1], foot_on_horizon[1]],\n", " [knee_on_horizon[2], foot_on_horizon[2]],\n", " color=self.knee_color)\n", " \n", " # Plot a circle as representing floor contact area/point.\n", " '''\n", " if self.it == 0:\n", " cir = Circle((foot_on_horizon[0], foot_on_horizon[1]), \n", " radius=0.05, \n", " color=(0.1, 0.1, 0.1),\n", " alpha=0.5) \n", " self.horizon_ax.add_patch(cir)\n", " art3d.pathpatch_2d_to_3d(cir, z=foot_on_horizon[2], zdir=\"z\")\n", " '''\n", "\n", " return foot_on_horizon\n", " \n", " def plot_on_horizon(self):\n", " '''\n", " Plot on horizon coordinate\n", " '''\n", " \n", " pos = self.Pos_SupportCentroidOnBody_Current \n", " R = self.Ori_SupportOnBody_Current\n", " \n", " # Calculate base body rectangle, 4 nodes, location on support coordinate.\n", " h0 = R_floor @ (R.T @ (self.arms[0].abad - pos) + Pos_floor)\n", " h1 = R_floor @ (R.T @ (self.arms[1].abad - pos) + Pos_floor)\n", " h2 = R_floor @ (R.T @ (self.arms[2].abad - pos) + Pos_floor)\n", " h3 = R_floor @ (R.T @ (self.arms[3].abad - pos) + Pos_floor)\n", " \n", " \n", " \n", " # Plot base body on horizon_ax\n", " self.horizon_ax.plot([h0[0], h1[0]],\n", " [h0[1], h1[1]],\n", " [h0[2], h1[2]],\n", " color=self.base_color)\n", " \n", " self.horizon_ax.plot([h0[0], h2[0]],\n", " [h0[1], h2[1]],\n", " [h0[2], h2[2]],\n", " color=self.base_color)\n", " \n", " self.horizon_ax.plot([h3[0], h1[0]],\n", " [h3[1], h1[1]],\n", " [h3[2], h1[2]],\n", " color=self.base_color)\n", " \n", " self.horizon_ax.plot([h3[0], h2[0]],\n", " [h3[1], h2[1]],\n", " [h3[2], h2[2]],\n", " color=self.base_color)\n", " \n", " # Plot 4 legs\n", " verts = [None, None, None, None]\n", " for i in range(4):\n", " verts[i] = self.plot_on_horizon_each_arm(self.arms[i])\n", " # self.horizon_ax.plot(*((h0 + h1 + h2 + h3)/4), marker='.', color = 'k')\n", " if self.it == 0:\n", " triangles = [[verts[0], verts[1], verts[2]], \n", " [verts[1], verts[2], verts[3]],\n", " [verts[2], verts[3], verts[0]],\n", " [verts[3], verts[0], verts[1]]]\n", "\n", " self.horizon_ax.add_collection(Poly3DCollection(triangles,\n", " edgecolor=(0.8, 0.8, 0.8),\n", " color = (0.5, 0.5, 0.5),\n", " alpha=0.2))\n", " # Update Error\n", " def update_error(self):\n", " # Estimator\n", " # Normal\n", " if self.support_normal_option == 'svd':\n", " normal = svd_regulated(np.array(A.subs(q_star), dtype=float))[0][:, -1] # This normal is normalized already\n", " self.Normal_SupportOnBody_Current = normal\n", " else:\n", "\n", " normal = np.cross(self.arms[2].foot - self.arms[3].foot, self.arms[1].foot - self.arms[3].foot)\n", " self.Normal_SupportOnBody_Current = normal / np.linalg.norm(normal)\n", "\n", " # Centroid\n", " self.Pos_SupportCentroidOnBody_Current = (self.arms[0].foot + self.arms[1].foot + self.arms[2].foot + self.arms[3].foot) / 4\n", "\n", " # EdgeY\n", " edgeY = self.get_edgeY() \n", " self.EdgeY_SupportOnBody_Current = edgeY / np.linalg.norm(edgeY)\n", "\n", " # After Ori Estimator, Ori prepare\n", " self.set_Ori_SupportOnBody_Current(self.EdgeY_SupportOnBody_Current, self.Normal_SupportOnBody_Current)\n", " self.Ori_BodyOnSupport_Current = self.Ori_SupportOnBody_Current.T\n", "\n", " # This need to be updated with real IMU, maybe from 4 points of body rectangle.\n", " # Maybe not need to be updated, since 4 points of body rectangles need to be\n", " # calculated by god view which need R_floor. Maybe use R_floor directly is just fine.\n", " self.IMU_BodyOnHorizon_Sim = R_floor @ self.Ori_SupportOnBody_Current.T\n", " self.Ori_BodyOnHorizon_Current = self.IMU_BodyOnHorizon_Sim\n", "\n", " self.Ori_SupportOnHorizon_Current = self.Ori_BodyOnHorizon_Current @ self.Ori_BodyOnSupport_Current.T\n", "\n", " print('Compare')\n", " print(self.Ori_SupportOnHorizon_Current)\n", " print(R_floor)\n", " self.set_Ori_BodyOnSupport_Target_byRotation(self.Ori_SupportOnHorizon_Current.T @ self.Ori_BodyOnHorizon_Target)\n", "\n", " # Pos prepare\n", " self.Pos_BodyOnSupport_Target = self.Ori_SupportOnHorizon_Current.T @ self.Pos_BodyOnHorizontalSupport_Target\n", " print('Pos_BodyOnSupport_Target: ', self.Pos_BodyOnSupport_Target)\n", " self.set_Pos_SupportOnBody_Target(self.Pos_BodyOnSupport_Target) # SE(3) conversion\n", "\n", " self.Ori_SupportOnBody_Error[0: 3] = self.Normal_SupportOnBody_Target - self.Normal_SupportOnBody_Current\n", " self.Ori_SupportOnBody_Error[3: 6] = self.EdgeY_SupportOnBody_Target - self.EdgeY_SupportOnBody_Current\n", " self.Pos_SupportOnBody_Error = self.Pos_SupportOnBody_Target - self.Pos_SupportCentroidOnBody_Current\n", " # Pos_SupportOnBody_Error = np.array([0.01, 0, 0])" ] }, { "cell_type": "code", "execution_count": 7, "id": "5748cfea", "metadata": { "colab": { "base_uri": "https://localhost:8080/", "height": 1000 }, "id": "5748cfea", "outputId": "107a71bb-8106-4034-f502-88ad694fcbde", "scrolled": false }, "outputs": [ { "data": { "application/javascript": [ "/* Put everything inside the global mpl namespace */\n", "/* global mpl */\n", "window.mpl = {};\n", "\n", "mpl.get_websocket_type = function () {\n", " if (typeof WebSocket !== 'undefined') {\n", " return WebSocket;\n", " } else if (typeof MozWebSocket !== 'undefined') {\n", " return MozWebSocket;\n", " } else {\n", " alert(\n", " 'Your browser does not have WebSocket support. ' +\n", " 'Please try Chrome, Safari or Firefox ≥ 6. ' +\n", " 'Firefox 4 and 5 are also supported but you ' +\n", " 'have to enable WebSockets in about:config.'\n", " );\n", " }\n", "};\n", "\n", "mpl.figure = function (figure_id, websocket, ondownload, parent_element) {\n", " this.id = figure_id;\n", "\n", " this.ws = websocket;\n", "\n", " this.supports_binary = this.ws.binaryType !== undefined;\n", "\n", " if (!this.supports_binary) {\n", " var warnings = document.getElementById('mpl-warnings');\n", " if (warnings) {\n", " warnings.style.display = 'block';\n", " warnings.textContent =\n", " 'This browser does not support binary websocket messages. ' +\n", " 'Performance may be slow.';\n", " }\n", " }\n", "\n", " this.imageObj = new Image();\n", "\n", " this.context = undefined;\n", " this.message = undefined;\n", " this.canvas = undefined;\n", " this.rubberband_canvas = undefined;\n", " this.rubberband_context = undefined;\n", " this.format_dropdown = undefined;\n", "\n", " this.image_mode = 'full';\n", "\n", " this.root = document.createElement('div');\n", " this.root.setAttribute('style', 'display: inline-block');\n", " this._root_extra_style(this.root);\n", "\n", " parent_element.appendChild(this.root);\n", "\n", " this._init_header(this);\n", " this._init_canvas(this);\n", " this._init_toolbar(this);\n", "\n", " var fig = this;\n", "\n", " this.waiting = false;\n", "\n", " this.ws.onopen = function () {\n", " fig.send_message('supports_binary', { value: fig.supports_binary });\n", " fig.send_message('send_image_mode', {});\n", " if (fig.ratio !== 1) {\n", " fig.send_message('set_device_pixel_ratio', {\n", " device_pixel_ratio: fig.ratio,\n", " });\n", " }\n", " fig.send_message('refresh', {});\n", " };\n", "\n", " this.imageObj.onload = function () {\n", " if (fig.image_mode === 'full') {\n", " // Full images could contain transparency (where diff images\n", " // almost always do), so we need to clear the canvas so that\n", " // there is no ghosting.\n", " fig.context.clearRect(0, 0, fig.canvas.width, fig.canvas.height);\n", " }\n", " fig.context.drawImage(fig.imageObj, 0, 0);\n", " };\n", "\n", " this.imageObj.onunload = function () {\n", " fig.ws.close();\n", " };\n", "\n", " this.ws.onmessage = this._make_on_message_function(this);\n", "\n", " this.ondownload = ondownload;\n", "};\n", "\n", "mpl.figure.prototype._init_header = function () {\n", " var titlebar = document.createElement('div');\n", " titlebar.classList =\n", " 'ui-dialog-titlebar ui-widget-header ui-corner-all ui-helper-clearfix';\n", " var titletext = document.createElement('div');\n", " titletext.classList = 'ui-dialog-title';\n", " titletext.setAttribute(\n", " 'style',\n", " 'width: 100%; text-align: center; padding: 3px;'\n", " );\n", " titlebar.appendChild(titletext);\n", " this.root.appendChild(titlebar);\n", " this.header = titletext;\n", "};\n", "\n", "mpl.figure.prototype._canvas_extra_style = function (_canvas_div) {};\n", "\n", "mpl.figure.prototype._root_extra_style = function (_canvas_div) {};\n", "\n", "mpl.figure.prototype._init_canvas = function () {\n", " var fig = this;\n", "\n", " var canvas_div = (this.canvas_div = document.createElement('div'));\n", " canvas_div.setAttribute('tabindex', '0');\n", " canvas_div.setAttribute(\n", " 'style',\n", " 'border: 1px solid #ddd;' +\n", " 'box-sizing: content-box;' +\n", " 'clear: both;' +\n", " 'min-height: 1px;' +\n", " 'min-width: 1px;' +\n", " 'outline: 0;' +\n", " 'overflow: hidden;' +\n", " 'position: relative;' +\n", " 'resize: both;' +\n", " 'z-index: 2;'\n", " );\n", "\n", " function on_keyboard_event_closure(name) {\n", " return function (event) {\n", " return fig.key_event(event, name);\n", " };\n", " }\n", "\n", " canvas_div.addEventListener(\n", " 'keydown',\n", " on_keyboard_event_closure('key_press')\n", " );\n", " canvas_div.addEventListener(\n", " 'keyup',\n", " on_keyboard_event_closure('key_release')\n", " );\n", "\n", " this._canvas_extra_style(canvas_div);\n", " this.root.appendChild(canvas_div);\n", "\n", " var canvas = (this.canvas = document.createElement('canvas'));\n", " canvas.classList.add('mpl-canvas');\n", " canvas.setAttribute(\n", " 'style',\n", " 'box-sizing: content-box;' +\n", " 'pointer-events: none;' +\n", " 'position: relative;' +\n", " 'z-index: 0;'\n", " );\n", "\n", " this.context = canvas.getContext('2d');\n", "\n", " var backingStore =\n", " this.context.backingStorePixelRatio ||\n", " this.context.webkitBackingStorePixelRatio ||\n", " this.context.mozBackingStorePixelRatio ||\n", " this.context.msBackingStorePixelRatio ||\n", " this.context.oBackingStorePixelRatio ||\n", " this.context.backingStorePixelRatio ||\n", " 1;\n", "\n", " this.ratio = (window.devicePixelRatio || 1) / backingStore;\n", "\n", " var rubberband_canvas = (this.rubberband_canvas = document.createElement(\n", " 'canvas'\n", " ));\n", " rubberband_canvas.setAttribute(\n", " 'style',\n", " 'box-sizing: content-box;' +\n", " 'left: 0;' +\n", " 'pointer-events: none;' +\n", " 'position: absolute;' +\n", " 'top: 0;' +\n", " 'z-index: 1;'\n", " );\n", "\n", " // Apply a ponyfill if ResizeObserver is not implemented by browser.\n", " if (this.ResizeObserver === undefined) {\n", " if (window.ResizeObserver !== undefined) {\n", " this.ResizeObserver = window.ResizeObserver;\n", " } else {\n", " var obs = _JSXTOOLS_RESIZE_OBSERVER({});\n", " this.ResizeObserver = obs.ResizeObserver;\n", " }\n", " }\n", "\n", " this.resizeObserverInstance = new this.ResizeObserver(function (entries) {\n", " var nentries = entries.length;\n", " for (var i = 0; i < nentries; i++) {\n", " var entry = entries[i];\n", " var width, height;\n", " if (entry.contentBoxSize) {\n", " if (entry.contentBoxSize instanceof Array) {\n", " // Chrome 84 implements new version of spec.\n", " width = entry.contentBoxSize[0].inlineSize;\n", " height = entry.contentBoxSize[0].blockSize;\n", " } else {\n", " // Firefox implements old version of spec.\n", " width = entry.contentBoxSize.inlineSize;\n", " height = entry.contentBoxSize.blockSize;\n", " }\n", " } else {\n", " // Chrome <84 implements even older version of spec.\n", " width = entry.contentRect.width;\n", " height = entry.contentRect.height;\n", " }\n", "\n", " // Keep the size of the canvas and rubber band canvas in sync with\n", " // the canvas container.\n", " if (entry.devicePixelContentBoxSize) {\n", " // Chrome 84 implements new version of spec.\n", " canvas.setAttribute(\n", " 'width',\n", " entry.devicePixelContentBoxSize[0].inlineSize\n", " );\n", " canvas.setAttribute(\n", " 'height',\n", " entry.devicePixelContentBoxSize[0].blockSize\n", " );\n", " } else {\n", " canvas.setAttribute('width', width * fig.ratio);\n", " canvas.setAttribute('height', height * fig.ratio);\n", " }\n", " /* This rescales the canvas back to display pixels, so that it\n", " * appears correct on HiDPI screens. */\n", " canvas.style.width = width + 'px';\n", " canvas.style.height = height + 'px';\n", "\n", " rubberband_canvas.setAttribute('width', width);\n", " rubberband_canvas.setAttribute('height', height);\n", "\n", " // And update the size in Python. We ignore the initial 0/0 size\n", " // that occurs as the element is placed into the DOM, which should\n", " // otherwise not happen due to the minimum size styling.\n", " if (fig.ws.readyState == 1 && width != 0 && height != 0) {\n", " fig.request_resize(width, height);\n", " }\n", " }\n", " });\n", " this.resizeObserverInstance.observe(canvas_div);\n", "\n", " function on_mouse_event_closure(name) {\n", " /* User Agent sniffing is bad, but WebKit is busted:\n", " * https://bugs.webkit.org/show_bug.cgi?id=144526\n", " * https://bugs.webkit.org/show_bug.cgi?id=181818\n", " * The worst that happens here is that they get an extra browser\n", " * selection when dragging, if this check fails to catch them.\n", " */\n", " var UA = navigator.userAgent;\n", " var isWebKit = /AppleWebKit/.test(UA) && !/Chrome/.test(UA);\n", " if(isWebKit) {\n", " return function (event) {\n", " /* This prevents the web browser from automatically changing to\n", " * the text insertion cursor when the button is pressed. We\n", " * want to control all of the cursor setting manually through\n", " * the 'cursor' event from matplotlib */\n", " event.preventDefault()\n", " return fig.mouse_event(event, name);\n", " };\n", " } else {\n", " return function (event) {\n", " return fig.mouse_event(event, name);\n", " };\n", " }\n", " }\n", "\n", " canvas_div.addEventListener(\n", " 'mousedown',\n", " on_mouse_event_closure('button_press')\n", " );\n", " canvas_div.addEventListener(\n", " 'mouseup',\n", " on_mouse_event_closure('button_release')\n", " );\n", " canvas_div.addEventListener(\n", " 'dblclick',\n", " on_mouse_event_closure('dblclick')\n", " );\n", " // Throttle sequential mouse events to 1 every 20ms.\n", " canvas_div.addEventListener(\n", " 'mousemove',\n", " on_mouse_event_closure('motion_notify')\n", " );\n", "\n", " canvas_div.addEventListener(\n", " 'mouseenter',\n", " on_mouse_event_closure('figure_enter')\n", " );\n", " canvas_div.addEventListener(\n", " 'mouseleave',\n", " on_mouse_event_closure('figure_leave')\n", " );\n", "\n", " canvas_div.addEventListener('wheel', function (event) {\n", " if (event.deltaY < 0) {\n", " event.step = 1;\n", " } else {\n", " event.step = -1;\n", " }\n", " on_mouse_event_closure('scroll')(event);\n", " });\n", "\n", " canvas_div.appendChild(canvas);\n", " canvas_div.appendChild(rubberband_canvas);\n", "\n", " this.rubberband_context = rubberband_canvas.getContext('2d');\n", " this.rubberband_context.strokeStyle = '#000000';\n", "\n", " this._resize_canvas = function (width, height, forward) {\n", " if (forward) {\n", " canvas_div.style.width = width + 'px';\n", " canvas_div.style.height = height + 'px';\n", " }\n", " };\n", "\n", " // Disable right mouse context menu.\n", " canvas_div.addEventListener('contextmenu', function (_e) {\n", " event.preventDefault();\n", " return false;\n", " });\n", "\n", " function set_focus() {\n", " canvas.focus();\n", " canvas_div.focus();\n", " }\n", "\n", " window.setTimeout(set_focus, 100);\n", "};\n", "\n", "mpl.figure.prototype._init_toolbar = function () {\n", " var fig = this;\n", "\n", " var toolbar = document.createElement('div');\n", " toolbar.classList = 'mpl-toolbar';\n", " this.root.appendChild(toolbar);\n", "\n", " function on_click_closure(name) {\n", " return function (_event) {\n", " return fig.toolbar_button_onclick(name);\n", " };\n", " }\n", "\n", " function on_mouseover_closure(tooltip) {\n", " return function (event) {\n", " if (!event.currentTarget.disabled) {\n", " return fig.toolbar_button_onmouseover(tooltip);\n", " }\n", " };\n", " }\n", "\n", " fig.buttons = {};\n", " var buttonGroup = document.createElement('div');\n", " buttonGroup.classList = 'mpl-button-group';\n", " for (var toolbar_ind in mpl.toolbar_items) {\n", " var name = mpl.toolbar_items[toolbar_ind][0];\n", " var tooltip = mpl.toolbar_items[toolbar_ind][1];\n", " var image = mpl.toolbar_items[toolbar_ind][2];\n", " var method_name = mpl.toolbar_items[toolbar_ind][3];\n", "\n", " if (!name) {\n", " /* Instead of a spacer, we start a new button group. */\n", " if (buttonGroup.hasChildNodes()) {\n", " toolbar.appendChild(buttonGroup);\n", " }\n", " buttonGroup = document.createElement('div');\n", " buttonGroup.classList = 'mpl-button-group';\n", " continue;\n", " }\n", "\n", " var button = (fig.buttons[name] = document.createElement('button'));\n", " button.classList = 'mpl-widget';\n", " button.setAttribute('role', 'button');\n", " button.setAttribute('aria-disabled', 'false');\n", " button.addEventListener('click', on_click_closure(method_name));\n", " button.addEventListener('mouseover', on_mouseover_closure(tooltip));\n", "\n", " var icon_img = document.createElement('img');\n", " icon_img.src = '_images/' + image + '.png';\n", " icon_img.srcset = '_images/' + image + '_large.png 2x';\n", " icon_img.alt = tooltip;\n", " button.appendChild(icon_img);\n", "\n", " buttonGroup.appendChild(button);\n", " }\n", "\n", " if (buttonGroup.hasChildNodes()) {\n", " toolbar.appendChild(buttonGroup);\n", " }\n", "\n", " var fmt_picker = document.createElement('select');\n", " fmt_picker.classList = 'mpl-widget';\n", " toolbar.appendChild(fmt_picker);\n", " this.format_dropdown = fmt_picker;\n", "\n", " for (var ind in mpl.extensions) {\n", " var fmt = mpl.extensions[ind];\n", " var option = document.createElement('option');\n", " option.selected = fmt === mpl.default_extension;\n", " option.innerHTML = fmt;\n", " fmt_picker.appendChild(option);\n", " }\n", "\n", " var status_bar = document.createElement('span');\n", " status_bar.classList = 'mpl-message';\n", " toolbar.appendChild(status_bar);\n", " this.message = status_bar;\n", "};\n", "\n", "mpl.figure.prototype.request_resize = function (x_pixels, y_pixels) {\n", " // Request matplotlib to resize the figure. Matplotlib will then trigger a resize in the client,\n", " // which will in turn request a refresh of the image.\n", " this.send_message('resize', { width: x_pixels, height: y_pixels });\n", "};\n", "\n", "mpl.figure.prototype.send_message = function (type, properties) {\n", " properties['type'] = type;\n", " properties['figure_id'] = this.id;\n", " this.ws.send(JSON.stringify(properties));\n", "};\n", "\n", "mpl.figure.prototype.send_draw_message = function () {\n", " if (!this.waiting) {\n", " this.waiting = true;\n", " this.ws.send(JSON.stringify({ type: 'draw', figure_id: this.id }));\n", " }\n", "};\n", "\n", "mpl.figure.prototype.handle_save = function (fig, _msg) {\n", " var format_dropdown = fig.format_dropdown;\n", " var format = format_dropdown.options[format_dropdown.selectedIndex].value;\n", " fig.ondownload(fig, format);\n", "};\n", "\n", "mpl.figure.prototype.handle_resize = function (fig, msg) {\n", " var size = msg['size'];\n", " if (size[0] !== fig.canvas.width || size[1] !== fig.canvas.height) {\n", " fig._resize_canvas(size[0], size[1], msg['forward']);\n", " fig.send_message('refresh', {});\n", " }\n", "};\n", "\n", "mpl.figure.prototype.handle_rubberband = function (fig, msg) {\n", " var x0 = msg['x0'] / fig.ratio;\n", " var y0 = (fig.canvas.height - msg['y0']) / fig.ratio;\n", " var x1 = msg['x1'] / fig.ratio;\n", " var y1 = (fig.canvas.height - msg['y1']) / fig.ratio;\n", " x0 = Math.floor(x0) + 0.5;\n", " y0 = Math.floor(y0) + 0.5;\n", " x1 = Math.floor(x1) + 0.5;\n", " y1 = Math.floor(y1) + 0.5;\n", " var min_x = Math.min(x0, x1);\n", " var min_y = Math.min(y0, y1);\n", " var width = Math.abs(x1 - x0);\n", " var height = Math.abs(y1 - y0);\n", "\n", " fig.rubberband_context.clearRect(\n", " 0,\n", " 0,\n", " fig.canvas.width / fig.ratio,\n", " fig.canvas.height / fig.ratio\n", " );\n", "\n", " fig.rubberband_context.strokeRect(min_x, min_y, width, height);\n", "};\n", "\n", "mpl.figure.prototype.handle_figure_label = function (fig, msg) {\n", " // Updates the figure title.\n", " fig.header.textContent = msg['label'];\n", "};\n", "\n", "mpl.figure.prototype.handle_cursor = function (fig, msg) {\n", " fig.canvas_div.style.cursor = msg['cursor'];\n", "};\n", "\n", "mpl.figure.prototype.handle_message = function (fig, msg) {\n", " fig.message.textContent = msg['message'];\n", "};\n", "\n", "mpl.figure.prototype.handle_draw = function (fig, _msg) {\n", " // Request the server to send over a new figure.\n", " fig.send_draw_message();\n", "};\n", "\n", "mpl.figure.prototype.handle_image_mode = function (fig, msg) {\n", " fig.image_mode = msg['mode'];\n", "};\n", "\n", "mpl.figure.prototype.handle_history_buttons = function (fig, msg) {\n", " for (var key in msg) {\n", " if (!(key in fig.buttons)) {\n", " continue;\n", " }\n", " fig.buttons[key].disabled = !msg[key];\n", " fig.buttons[key].setAttribute('aria-disabled', !msg[key]);\n", " }\n", "};\n", "\n", "mpl.figure.prototype.handle_navigate_mode = function (fig, msg) {\n", " if (msg['mode'] === 'PAN') {\n", " fig.buttons['Pan'].classList.add('active');\n", " fig.buttons['Zoom'].classList.remove('active');\n", " } else if (msg['mode'] === 'ZOOM') {\n", " fig.buttons['Pan'].classList.remove('active');\n", " fig.buttons['Zoom'].classList.add('active');\n", " } else {\n", " fig.buttons['Pan'].classList.remove('active');\n", " fig.buttons['Zoom'].classList.remove('active');\n", " }\n", "};\n", "\n", "mpl.figure.prototype.updated_canvas_event = function () {\n", " // Called whenever the canvas gets updated.\n", " this.send_message('ack', {});\n", "};\n", "\n", "// A function to construct a web socket function for onmessage handling.\n", "// Called in the figure constructor.\n", "mpl.figure.prototype._make_on_message_function = function (fig) {\n", " return function socket_on_message(evt) {\n", " if (evt.data instanceof Blob) {\n", " var img = evt.data;\n", " if (img.type !== 'image/png') {\n", " /* FIXME: We get \"Resource interpreted as Image but\n", " * transferred with MIME type text/plain:\" errors on\n", " * Chrome. But how to set the MIME type? It doesn't seem\n", " * to be part of the websocket stream */\n", " img.type = 'image/png';\n", " }\n", "\n", " /* Free the memory for the previous frames */\n", " if (fig.imageObj.src) {\n", " (window.URL || window.webkitURL).revokeObjectURL(\n", " fig.imageObj.src\n", " );\n", " }\n", "\n", " fig.imageObj.src = (window.URL || window.webkitURL).createObjectURL(\n", " img\n", " );\n", " fig.updated_canvas_event();\n", " fig.waiting = false;\n", " return;\n", " } else if (\n", " typeof evt.data === 'string' &&\n", " evt.data.slice(0, 21) === 'data:image/png;base64'\n", " ) {\n", " fig.imageObj.src = evt.data;\n", " fig.updated_canvas_event();\n", " fig.waiting = false;\n", " return;\n", " }\n", "\n", " var msg = JSON.parse(evt.data);\n", " var msg_type = msg['type'];\n", "\n", " // Call the \"handle_{type}\" callback, which takes\n", " // the figure and JSON message as its only arguments.\n", " try {\n", " var callback = fig['handle_' + msg_type];\n", " } catch (e) {\n", " console.log(\n", " \"No handler for the '\" + msg_type + \"' message type: \",\n", " msg\n", " );\n", " return;\n", " }\n", "\n", " if (callback) {\n", " try {\n", " // console.log(\"Handling '\" + msg_type + \"' message: \", msg);\n", " callback(fig, msg);\n", " } catch (e) {\n", " console.log(\n", " \"Exception inside the 'handler_\" + msg_type + \"' callback:\",\n", " e,\n", " e.stack,\n", " msg\n", " );\n", " }\n", " }\n", " };\n", "};\n", "\n", "\n", "/*\n", " * return a copy of an object with only non-object keys\n", " * we need this to avoid circular references\n", " * https://stackoverflow.com/a/24161582/3208463\n", " */\n", "function simpleKeys(original) {\n", " return Object.keys(original).reduce(function (obj, key) {\n", " if (typeof original[key] !== 'object') {\n", " obj[key] = original[key];\n", " }\n", " return obj;\n", " }, {});\n", "}\n", "\n", "mpl.figure.prototype.mouse_event = function (event, name) {\n", " if (name === 'button_press') {\n", " this.canvas.focus();\n", " this.canvas_div.focus();\n", " }\n", "\n", " // from https://stackoverflow.com/q/1114465\n", " var boundingRect = this.canvas.getBoundingClientRect();\n", " var x = (event.clientX - boundingRect.left) * this.ratio;\n", " var y = (event.clientY - boundingRect.top) * this.ratio;\n", "\n", " this.send_message(name, {\n", " x: x,\n", " y: y,\n", " button: event.button,\n", " step: event.step,\n", " guiEvent: simpleKeys(event),\n", " });\n", "\n", " return false;\n", "};\n", "\n", "mpl.figure.prototype._key_event_extra = function (_event, _name) {\n", " // Handle any extra behaviour associated with a key event\n", "};\n", "\n", "mpl.figure.prototype.key_event = function (event, name) {\n", " // Prevent repeat events\n", " if (name === 'key_press') {\n", " if (event.key === this._key) {\n", " return;\n", " } else {\n", " this._key = event.key;\n", " }\n", " }\n", " if (name === 'key_release') {\n", " this._key = null;\n", " }\n", "\n", " var value = '';\n", " if (event.ctrlKey && event.key !== 'Control') {\n", " value += 'ctrl+';\n", " }\n", " else if (event.altKey && event.key !== 'Alt') {\n", " value += 'alt+';\n", " }\n", " else if (event.shiftKey && event.key !== 'Shift') {\n", " value += 'shift+';\n", " }\n", "\n", " value += 'k' + event.key;\n", "\n", " this._key_event_extra(event, name);\n", "\n", " this.send_message(name, { key: value, guiEvent: simpleKeys(event) });\n", " return false;\n", "};\n", "\n", "mpl.figure.prototype.toolbar_button_onclick = function (name) {\n", " if (name === 'download') {\n", " this.handle_save(this, null);\n", " } else {\n", " this.send_message('toolbar_button', { name: name });\n", " }\n", "};\n", "\n", "mpl.figure.prototype.toolbar_button_onmouseover = function (tooltip) {\n", " this.message.textContent = tooltip;\n", "};\n", "\n", "///////////////// REMAINING CONTENT GENERATED BY embed_js.py /////////////////\n", "// prettier-ignore\n", "var _JSXTOOLS_RESIZE_OBSERVER=function(A){var t,i=new WeakMap,n=new WeakMap,a=new WeakMap,r=new WeakMap,o=new Set;function s(e){if(!(this instanceof s))throw new TypeError(\"Constructor requires 'new' operator\");i.set(this,e)}function h(){throw new TypeError(\"Function is not a constructor\")}function c(e,t,i,n){e=0 in arguments?Number(arguments[0]):0,t=1 in arguments?Number(arguments[1]):0,i=2 in arguments?Number(arguments[2]):0,n=3 in arguments?Number(arguments[3]):0,this.right=(this.x=this.left=e)+(this.width=i),this.bottom=(this.y=this.top=t)+(this.height=n),Object.freeze(this)}function d(){t=requestAnimationFrame(d);var s=new WeakMap,p=new Set;o.forEach((function(t){r.get(t).forEach((function(i){var r=t instanceof window.SVGElement,o=a.get(t),d=r?0:parseFloat(o.paddingTop),f=r?0:parseFloat(o.paddingRight),l=r?0:parseFloat(o.paddingBottom),u=r?0:parseFloat(o.paddingLeft),g=r?0:parseFloat(o.borderTopWidth),m=r?0:parseFloat(o.borderRightWidth),w=r?0:parseFloat(o.borderBottomWidth),b=u+f,F=d+l,v=(r?0:parseFloat(o.borderLeftWidth))+m,W=g+w,y=r?0:t.offsetHeight-W-t.clientHeight,E=r?0:t.offsetWidth-v-t.clientWidth,R=b+v,z=F+W,M=r?t.width:parseFloat(o.width)-R-E,O=r?t.height:parseFloat(o.height)-z-y;if(n.has(t)){var k=n.get(t);if(k[0]===M&&k[1]===O)return}n.set(t,[M,O]);var S=Object.create(h.prototype);S.target=t,S.contentRect=new c(u,d,M,O),s.has(i)||(s.set(i,[]),p.add(i)),s.get(i).push(S)}))})),p.forEach((function(e){i.get(e).call(e,s.get(e),e)}))}return s.prototype.observe=function(i){if(i instanceof window.Element){r.has(i)||(r.set(i,new Set),o.add(i),a.set(i,window.getComputedStyle(i)));var n=r.get(i);n.has(this)||n.add(this),cancelAnimationFrame(t),t=requestAnimationFrame(d)}},s.prototype.unobserve=function(i){if(i instanceof window.Element&&r.has(i)){var n=r.get(i);n.has(this)&&(n.delete(this),n.size||(r.delete(i),o.delete(i))),n.size||r.delete(i),o.size||cancelAnimationFrame(t)}},A.DOMRectReadOnly=c,A.ResizeObserver=s,A.ResizeObserverEntry=h,A}; // eslint-disable-line\n", "mpl.toolbar_items = [[\"Home\", \"Reset original view\", \"fa fa-home\", \"home\"], [\"Back\", \"Back to previous view\", \"fa fa-arrow-left\", \"back\"], [\"Forward\", \"Forward to next view\", \"fa fa-arrow-right\", \"forward\"], [\"\", \"\", \"\", \"\"], [\"Pan\", \"Left button pans, Right button zooms\\nx/y fixes axis, CTRL fixes aspect\", \"fa fa-arrows\", \"pan\"], [\"Zoom\", \"Zoom to rectangle\\nx/y fixes axis\", \"fa fa-square-o\", \"zoom\"], [\"\", \"\", \"\", \"\"], [\"Download\", \"Download plot\", \"fa fa-floppy-o\", \"download\"]];\n", "\n", "mpl.extensions = [\"eps\", \"jpeg\", \"pgf\", \"pdf\", \"png\", \"ps\", \"raw\", \"svg\", \"tif\", \"webp\"];\n", "\n", "mpl.default_extension = \"png\";/* global mpl */\n", "\n", "var comm_websocket_adapter = function (comm) {\n", " // Create a \"websocket\"-like object which calls the given IPython comm\n", " // object with the appropriate methods. Currently this is a non binary\n", " // socket, so there is still some room for performance tuning.\n", " var ws = {};\n", "\n", " ws.binaryType = comm.kernel.ws.binaryType;\n", " ws.readyState = comm.kernel.ws.readyState;\n", " function updateReadyState(_event) {\n", " if (comm.kernel.ws) {\n", " ws.readyState = comm.kernel.ws.readyState;\n", " } else {\n", " ws.readyState = 3; // Closed state.\n", " }\n", " }\n", " comm.kernel.ws.addEventListener('open', updateReadyState);\n", " comm.kernel.ws.addEventListener('close', updateReadyState);\n", " comm.kernel.ws.addEventListener('error', updateReadyState);\n", "\n", " ws.close = function () {\n", " comm.close();\n", " };\n", " ws.send = function (m) {\n", " //console.log('sending', m);\n", " comm.send(m);\n", " };\n", " // Register the callback with on_msg.\n", " comm.on_msg(function (msg) {\n", " //console.log('receiving', msg['content']['data'], msg);\n", " var data = msg['content']['data'];\n", " if (data['blob'] !== undefined) {\n", " data = {\n", " data: new Blob(msg['buffers'], { type: data['blob'] }),\n", " };\n", " }\n", " // Pass the mpl event to the overridden (by mpl) onmessage function.\n", " ws.onmessage(data);\n", " });\n", " return ws;\n", "};\n", "\n", "mpl.mpl_figure_comm = function (comm, msg) {\n", " // This is the function which gets called when the mpl process\n", " // starts-up an IPython Comm through the \"matplotlib\" channel.\n", "\n", " var id = msg.content.data.id;\n", " // Get hold of the div created by the display call when the Comm\n", " // socket was opened in Python.\n", " var element = document.getElementById(id);\n", " var ws_proxy = comm_websocket_adapter(comm);\n", "\n", " function ondownload(figure, _format) {\n", " window.open(figure.canvas.toDataURL());\n", " }\n", "\n", " var fig = new mpl.figure(id, ws_proxy, ondownload, element);\n", "\n", " // Call onopen now - mpl needs it, as it is assuming we've passed it a real\n", " // web socket which is closed, not our websocket->open comm proxy.\n", " ws_proxy.onopen();\n", "\n", " fig.parent_element = element;\n", " fig.cell_info = mpl.find_output_cell(\"
\");\n", " if (!fig.cell_info) {\n", " console.error('Failed to find cell for figure', id, fig);\n", " return;\n", " }\n", " fig.cell_info[0].output_area.element.on(\n", " 'cleared',\n", " { fig: fig },\n", " fig._remove_fig_handler\n", " );\n", "};\n", "\n", "mpl.figure.prototype.handle_close = function (fig, msg) {\n", " var width = fig.canvas.width / fig.ratio;\n", " fig.cell_info[0].output_area.element.off(\n", " 'cleared',\n", " fig._remove_fig_handler\n", " );\n", " fig.resizeObserverInstance.unobserve(fig.canvas_div);\n", "\n", " // Update the output cell to use the data from the current canvas.\n", " fig.push_to_output();\n", " var dataURL = fig.canvas.toDataURL();\n", " // Re-enable the keyboard manager in IPython - without this line, in FF,\n", " // the notebook keyboard shortcuts fail.\n", " IPython.keyboard_manager.enable();\n", " fig.parent_element.innerHTML =\n", " '';\n", " fig.close_ws(fig, msg);\n", "};\n", "\n", "mpl.figure.prototype.close_ws = function (fig, msg) {\n", " fig.send_message('closing', msg);\n", " // fig.ws.close()\n", "};\n", "\n", "mpl.figure.prototype.push_to_output = function (_remove_interactive) {\n", " // Turn the data on the canvas into data in the output cell.\n", " var width = this.canvas.width / this.ratio;\n", " var dataURL = this.canvas.toDataURL();\n", " this.cell_info[1]['text/html'] =\n", " '';\n", "};\n", "\n", "mpl.figure.prototype.updated_canvas_event = function () {\n", " // Tell IPython that the notebook contents must change.\n", " IPython.notebook.set_dirty(true);\n", " this.send_message('ack', {});\n", " var fig = this;\n", " // Wait a second, then push the new image to the DOM so\n", " // that it is saved nicely (might be nice to debounce this).\n", " setTimeout(function () {\n", " fig.push_to_output();\n", " }, 1000);\n", "};\n", "\n", "mpl.figure.prototype._init_toolbar = function () {\n", " var fig = this;\n", "\n", " var toolbar = document.createElement('div');\n", " toolbar.classList = 'btn-toolbar';\n", " this.root.appendChild(toolbar);\n", "\n", " function on_click_closure(name) {\n", " return function (_event) {\n", " return fig.toolbar_button_onclick(name);\n", " };\n", " }\n", "\n", " function on_mouseover_closure(tooltip) {\n", " return function (event) {\n", " if (!event.currentTarget.disabled) {\n", " return fig.toolbar_button_onmouseover(tooltip);\n", " }\n", " };\n", " }\n", "\n", " fig.buttons = {};\n", " var buttonGroup = document.createElement('div');\n", " buttonGroup.classList = 'btn-group';\n", " var button;\n", " for (var toolbar_ind in mpl.toolbar_items) {\n", " var name = mpl.toolbar_items[toolbar_ind][0];\n", " var tooltip = mpl.toolbar_items[toolbar_ind][1];\n", " var image = mpl.toolbar_items[toolbar_ind][2];\n", " var method_name = mpl.toolbar_items[toolbar_ind][3];\n", "\n", " if (!name) {\n", " /* Instead of a spacer, we start a new button group. */\n", " if (buttonGroup.hasChildNodes()) {\n", " toolbar.appendChild(buttonGroup);\n", " }\n", " buttonGroup = document.createElement('div');\n", " buttonGroup.classList = 'btn-group';\n", " continue;\n", " }\n", "\n", " button = fig.buttons[name] = document.createElement('button');\n", " button.classList = 'btn btn-default';\n", " button.href = '#';\n", " button.title = name;\n", " button.innerHTML = '';\n", " button.addEventListener('click', on_click_closure(method_name));\n", " button.addEventListener('mouseover', on_mouseover_closure(tooltip));\n", " buttonGroup.appendChild(button);\n", " }\n", "\n", " if (buttonGroup.hasChildNodes()) {\n", " toolbar.appendChild(buttonGroup);\n", " }\n", "\n", " // Add the status bar.\n", " var status_bar = document.createElement('span');\n", " status_bar.classList = 'mpl-message pull-right';\n", " toolbar.appendChild(status_bar);\n", " this.message = status_bar;\n", "\n", " // Add the close button to the window.\n", " var buttongrp = document.createElement('div');\n", " buttongrp.classList = 'btn-group inline pull-right';\n", " button = document.createElement('button');\n", " button.classList = 'btn btn-mini btn-primary';\n", " button.href = '#';\n", " button.title = 'Stop Interaction';\n", " button.innerHTML = '';\n", " button.addEventListener('click', function (_evt) {\n", " fig.handle_close(fig, {});\n", " });\n", " button.addEventListener(\n", " 'mouseover',\n", " on_mouseover_closure('Stop Interaction')\n", " );\n", " buttongrp.appendChild(button);\n", " var titlebar = this.root.querySelector('.ui-dialog-titlebar');\n", " titlebar.insertBefore(buttongrp, titlebar.firstChild);\n", "};\n", "\n", "mpl.figure.prototype._remove_fig_handler = function (event) {\n", " var fig = event.data.fig;\n", " if (event.target !== this) {\n", " // Ignore bubbled events from children.\n", " return;\n", " }\n", " fig.close_ws(fig, {});\n", "};\n", "\n", "mpl.figure.prototype._root_extra_style = function (el) {\n", " el.style.boxSizing = 'content-box'; // override notebook setting of border-box.\n", "};\n", "\n", "mpl.figure.prototype._canvas_extra_style = function (el) {\n", " // this is important to make the div 'focusable\n", " el.setAttribute('tabindex', 0);\n", " // reach out to IPython and tell the keyboard manager to turn it's self\n", " // off when our div gets focus\n", "\n", " // location in version 3\n", " if (IPython.notebook.keyboard_manager) {\n", " IPython.notebook.keyboard_manager.register_events(el);\n", " } else {\n", " // location in version 2\n", " IPython.keyboard_manager.register_events(el);\n", " }\n", "};\n", "\n", "mpl.figure.prototype._key_event_extra = function (event, _name) {\n", " // Check for shift+enter\n", " if (event.shiftKey && event.which === 13) {\n", " this.canvas_div.blur();\n", " // select the cell after this one\n", " var index = IPython.notebook.find_cell_index(this.cell_info[0]);\n", " IPython.notebook.select(index + 1);\n", " }\n", "};\n", "\n", "mpl.figure.prototype.handle_save = function (fig, _msg) {\n", " fig.ondownload(fig, null);\n", "};\n", "\n", "mpl.find_output_cell = function (html_output) {\n", " // Return the cell and output element which can be found *uniquely* in the notebook.\n", " // Note - this is a bit hacky, but it is done because the \"notebook_saving.Notebook\"\n", " // IPython event is triggered only after the cells have been serialised, which for\n", " // our purposes (turning an active figure into a static one), is too late.\n", " var cells = IPython.notebook.get_cells();\n", " var ncells = cells.length;\n", " for (var i = 0; i < ncells; i++) {\n", " var cell = cells[i];\n", " if (cell.cell_type === 'code') {\n", " for (var j = 0; j < cell.output_area.outputs.length; j++) {\n", " var data = cell.output_area.outputs[j];\n", " if (data.data) {\n", " // IPython >= 3 moved mimebundle to data attribute of output\n", " data = data.data;\n", " }\n", " if (data['text/html'] === html_output) {\n", " return [cell, data, j];\n", " }\n", " }\n", " }\n", " }\n", "};\n", "\n", "// Register the function which deals with the matplotlib target/channel.\n", "// The kernel may be null if the page has been refreshed.\n", "if (IPython.notebook.kernel !== null) {\n", " IPython.notebook.kernel.comm_manager.register_target(\n", " 'matplotlib',\n", " mpl.mpl_figure_comm\n", " );\n", "}\n" ], "text/plain": [ "" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/html": [ "" ], "text/plain": [ "" ] }, "metadata": {}, "output_type": "display_data" }, { "name": "stdout", "output_type": "stream", "text": [ "i: 0\n", "Matrix([[0.200652405858808]])\n", "Matrix([[0.113874846476112]])\n", "Matrix([[0.205565960700685]])\n", "Matrix([[0.0556261287052016]])\n", "Matrix([[0.205937189440296]])\n", "Matrix([[0.0556261287052016]])\n", "Compare\n", "[[ 8.66025400e-01 2.08929909e-17 -5.00000000e-01]\n", " [ 2.01497592e-21 1.00000000e+00 -4.17287452e-17]\n", " [ 5.00000000e-01 -3.61776890e-17 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [ 1.30000000e-01 -9.40619915e-18 2.25166604e-01]\n", "i: 1\n", "Matrix([[0.200464197426834]])\n", "Matrix([[0.113630557404181]])\n", "Matrix([[0.205358968165558]])\n", "Matrix([[0.0556366211930191]])\n", "Matrix([[0.205732193354102]])\n", "Matrix([[0.0557051759365938]])\n", "Compare\n", "[[ 8.66025400e-01 -1.88842018e-17 -5.00000000e-01]\n", " [ 5.01891646e-19 1.00000000e+00 3.79858419e-17]\n", " [ 5.00000000e-01 3.29499332e-17 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [1.30000000e-01 8.56698262e-18 2.25166604e-01]\n", "i: 2\n", "Matrix([[0.199785565147066]])\n", "Matrix([[0.112838490615058]])\n", "Matrix([[0.204665682479924]])\n", "Matrix([[0.0556692671887738]])\n", "Matrix([[0.204959983456642]])\n", "Matrix([[0.0559524087384013]])\n", "Compare\n", "[[ 8.66025400e-01 3.46676086e-17 -5.00000000e-01]\n", " [ 1.23558295e-18 1.00000000e+00 -7.18704038e-17]\n", " [ 5.00000000e-01 -6.24049710e-17 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [ 1.30000000e-01 -1.62252925e-17 2.25166604e-01]\n", "i: 3\n", "Matrix([[0.198591051556393]])\n", "Matrix([[0.111699293718025]])\n", "Matrix([[0.203593181867003]])\n", "Matrix([[0.0557151078876275]])\n", "Matrix([[0.203485639625737]])\n", "Matrix([[0.0562810757471112]])\n", "Compare\n", "[[ 8.66025400e-01 1.56205193e-17 -5.00000000e-01]\n", " [-8.69233573e-19 1.00000000e+00 -3.35792459e-17]\n", " [ 5.00000000e-01 -2.88602560e-17 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [ 1.30000000e-01 -7.50366657e-18 2.25166604e-01]\n", "i: 4\n", "Matrix([[0.197196260789123]])\n", "Matrix([[0.110688127890502]])\n", "Matrix([[0.202549627925695]])\n", "Matrix([[0.0557605326487339]])\n", "Matrix([[0.201616903223953]])\n", "Matrix([[0.0565494426098182]])\n", "Compare\n", "[[ 8.66025400e-01 -7.96849266e-18 -5.00000000e-01]\n", " [ 1.54410448e-19 1.00000000e+00 1.08808800e-17]\n", " [ 5.00000000e-01 8.02577976e-18 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [1.30000000e-01 2.08670274e-18 2.25166604e-01]\n", "i: 5\n", "Matrix([[0.195978520992071]])\n", "Matrix([[0.110058019702853]])\n", "Matrix([[0.201849886684243]])\n", "Matrix([[0.0557972686342503]])\n", "Matrix([[0.199897177573393]])\n", "Matrix([[0.0567088889879161]])\n", "Compare\n", "[[ 8.66025400e-01 -5.50941382e-17 -5.00000000e-01]\n", " [ 1.03734162e-18 1.00000000e+00 1.12973066e-16]\n", " [ 5.00000000e-01 9.81308049e-17 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [1.30000000e-01 2.55140093e-17 2.25166604e-01]\n", "i: 6\n", "Matrix([[0.195102434476325]])\n", "Matrix([[0.109779513369715]])\n", "Matrix([[0.201524217506006]])\n", "Matrix([[0.0558220574068817]])\n", "Matrix([[0.198620772328339]])\n", "Matrix([[0.0567777478181446]])\n", "Compare\n", "[[ 8.66025400e-01 -4.98682638e-17 -5.00000000e-01]\n", " [ 8.90269652e-19 1.00000000e+00 1.00925728e-16]\n", " [ 5.00000000e-01 8.79179480e-17 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [1.30000000e-01 2.28586665e-17 2.25166604e-01]\n", "i: 7\n", "Matrix([[0.194540065791544]])\n", "Matrix([[0.109725189797687]])\n", "Matrix([[0.201447227105906]])\n", "Matrix([[0.0558371244751695]])\n", "Matrix([[0.197772390582585]])\n", "Matrix([[0.0567862238790339]])\n", "Compare\n", "[[ 8.66025400e-01 -5.88324198e-17 -5.00000000e-01]\n", " [-6.88817889e-19 1.00000000e+00 1.17533907e-16]\n", " [ 5.00000000e-01 1.01707860e-16 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [1.30000000e-01 2.64440436e-17 2.25166604e-01]\n", "i: 8\n", "Matrix([[0.194193100424057]])\n", "Matrix([[0.109771253721704]])\n", "Matrix([[0.201484424087798]])\n", "Matrix([[0.0558463265903783]])\n", "Matrix([[0.197227383912645]])\n", "Matrix([[0.0567615364009154]])\n", "Compare\n", "[[ 8.66025400e-01 -2.78186774e-17 -5.00000000e-01]\n", " [ 3.88832243e-19 1.00000000e+00 5.70632787e-17]\n", " [ 5.00000000e-01 4.92632927e-17 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [1.30000000e-01 1.28084561e-17 2.25166604e-01]\n", "i: 9\n", "Matrix([[0.193971915373273]])\n", "Matrix([[0.109835896331777]])\n", "Matrix([[0.201543007848993]])\n", "Matrix([[0.0558525473874137]])\n", "Matrix([[0.196865465949048]])\n", "Matrix([[0.0567240193961370]])\n", "Compare\n", "[[ 8.66025400e-01 1.49397716e-17 -5.00000000e-01]\n", " [ 7.54498114e-20 1.00000000e+00 -2.79872162e-17]\n", " [ 5.00000000e-01 -2.43343222e-17 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [ 1.30000000e-01 -6.32692376e-18 2.25166604e-01]\n", "i: 10\n", "Matrix([[0.193822549289113]])\n", "Matrix([[0.109883546237539]])\n", "Matrix([[0.201579645395266]])\n", "Matrix([[0.0558572831165495]])\n", "Matrix([[0.196609507011765]])\n", "Matrix([[0.0566865003985952]])\n", "Compare\n", "[[ 8.66025400e-01 1.07947130e-19 -5.00000000e-01]\n", " [-2.75657700e-19 1.00000000e+00 5.27394948e-19]\n", " [ 5.00000000e-01 4.16298299e-19 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [1.30000000e-01 1.08237558e-19 2.25166604e-01]\n", "i: 11\n", "Matrix([[0.193721120920278]])\n", "Matrix([[0.109908832453315]])\n", "Matrix([[0.201590528120759]])\n", "Matrix([[0.0558610046703323]])\n", "Matrix([[0.196428462651993]])\n", "Matrix([[0.0566561822469021]])\n", "Compare\n", "[[ 8.66025400e-01 3.63560368e-17 -5.00000000e-01]\n", " [ 7.74995627e-20 1.00000000e+00 -7.23523453e-17]\n", " [ 5.00000000e-01 -6.23253647e-17 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [ 1.30000000e-01 -1.62045948e-17 2.25166604e-01]\n", "i: 12\n", "Matrix([[0.193658138092893]])\n", "Matrix([[0.109919088433488]])\n", "Matrix([[0.201588524349653]])\n", "Matrix([[0.0558636413390986]])\n", "Matrix([[0.196313139018673]])\n", "Matrix([[0.0566359134666801]])\n", "Compare\n", "[[ 8.66025400e-01 -6.03511779e-17 -5.00000000e-01]\n", " [ 1.18883651e-19 1.00000000e+00 1.21020475e-16]\n", " [ 5.00000000e-01 1.04909824e-16 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [1.30000000e-01 2.72765543e-17 2.25166604e-01]\n", "i: 13\n", "Matrix([[0.193625328368193]])\n", "Matrix([[0.109922287899392]])\n", "Matrix([[0.201584433638831]])\n", "Matrix([[0.0558651879223760]])\n", "Matrix([[0.196252352027075]])\n", "Matrix([[0.0566250015227241]])\n", "Compare\n", "[[ 8.66025400e-01 2.70085048e-17 -5.00000000e-01]\n", " [-1.31503602e-20 1.00000000e+00 -5.39593163e-17]\n", " [ 5.00000000e-01 -4.66552947e-17 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [ 1.30000000e-01 -1.21303766e-17 2.25166604e-01]\n", "i: 14\n", "Matrix([[0.193611907101886]])\n", "Matrix([[0.109923037013635]])\n", "Matrix([[0.201582024733209]])\n", "Matrix([[0.0558658957167918]])\n", "Matrix([[0.196227325877905]])\n", "Matrix([[0.0566204650723440]])\n", "Compare\n", "[[ 8.66025400e-01 -6.26047161e-17 -5.00000000e-01]\n", " [-2.33229701e-20 1.00000000e+00 1.25169014e-16]\n", " [ 5.00000000e-01 1.08369388e-16 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [1.30000000e-01 2.81760408e-17 2.25166604e-01]\n", "i: 15\n", "Matrix([[0.193607794162576]])\n", "Matrix([[0.109923161305813]])\n", "Matrix([[0.201581137345032]])\n", "Matrix([[0.0558661458818032]])\n", "Matrix([[0.196219599928836]])\n", "Matrix([[0.0566190679235582]])\n", "Compare\n", "[[ 8.66025400e-01 -7.52298724e-17 -5.00000000e-01]\n", " [-1.98617141e-21 1.00000000e+00 1.50435162e-16]\n", " [ 5.00000000e-01 1.30281771e-16 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [1.30000000e-01 3.38732605e-17 2.25166604e-01]\n" ] }, { "name": "stdout", "output_type": "stream", "text": [ "i: 16\n", "Matrix([[0.193606882734149]])\n", "Matrix([[0.109923171256572]])\n", "Matrix([[0.201580913321427]])\n", "Matrix([[0.0558662064822084]])\n", "Matrix([[0.196217870954272]])\n", "Matrix([[0.0566187578856053]])\n", "Compare\n", "[[ 8.66025400e-01 -3.16182854e-17 -5.00000000e-01]\n", " [ 8.58154627e-22 1.00000000e+00 6.32401072e-17]\n", " [ 5.00000000e-01 5.47669875e-17 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [1.30000000e-01 1.42394168e-17 2.25166604e-01]\n", "i: 17\n", "Matrix([[0.193606741317178]])\n", "Matrix([[0.109923173128898]])\n", "Matrix([[0.201580874558851]])\n", "Matrix([[0.0558662200884570]])\n", "Matrix([[0.196217596162483]])\n", "Matrix([[0.0566187106701023]])\n", "Compare\n", "[[ 8.66025400e-01 -2.67896254e-17 -5.00000000e-01]\n", " [-7.08740581e-22 1.00000000e+00 5.35870702e-17]\n", " [ 5.00000000e-01 4.64057323e-17 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [1.30000000e-01 1.20654904e-17 2.25166604e-01]\n", "i: 18\n", "Matrix([[0.193606728196559]])\n", "Matrix([[0.109923172310622]])\n", "Matrix([[0.201580872004251]])\n", "Matrix([[0.0558662209414493]])\n", "Matrix([[0.196217569850389]])\n", "Matrix([[0.0566187060352217]])\n", "Compare\n", "[[ 8.66025400e-01 4.14219457e-18 -5.00000000e-01]\n", " [-6.16918064e-21 1.00000000e+00 -8.27898568e-18]\n", " [ 5.00000000e-01 -7.16409283e-18 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [ 1.30000000e-01 -1.86266414e-18 2.25166604e-01]\n", "i: 19\n", "Matrix([[0.193606740688201]])\n", "Matrix([[0.109923178705195]])\n", "Matrix([[0.201580883995420]])\n", "Matrix([[0.0558662303576911]])\n", "Matrix([[0.196217574854985]])\n", "Matrix([[0.0566187110900983]])\n", "Compare\n", "[[ 8.66025400e-01 -1.10289229e-16 -5.00000000e-01]\n", " [-1.74908901e-20 1.00000000e+00 2.20540429e-16]\n", " [ 5.00000000e-01 1.90997215e-16 8.66025400e-01]]\n", "[[ 0.8660254 0. -0.5 ]\n", " [ 0. 1. 0. ]\n", " [ 0.5 0. 0.8660254]]\n", "Pos_BodyOnSupport_Target: [1.30000000e-01 4.96592759e-17 2.25166604e-01]\n" ] } ], "source": [ "# Whole control Figure\n", "'''\n", " Top View \n", "\n", " 1 --- 0\n", " | |\n", " | |\n", " | |\n", " 3 --- 2\n", "'''\n", "\n", "%matplotlib notebook\n", "%matplotlib notebook\n", "plt.rcParams[\"figure.figsize\"] = (10,20)\n", "# The iter number\n", "num = 20\n", "\n", "# The init value of joints, q0 - q11\n", "\n", "q = np.array([pi/30, -pi/2, 4/5*pi,\n", " -pi/30, -pi/2, 2/3*pi,\n", " pi/30, -pi/2, 2/3*pi,\n", " -pi/30, -pi/2, 4/5*pi])\n", "\n", "\n", "\n", "robot = FourLegRobot(ThreeLinkArm(q[0:3], num, abad0, hip0, knee0, foot0),\n", " ThreeLinkArm(q[3:6], num, abad1, hip1, knee1, foot1),\n", " ThreeLinkArm(q[6:9], num, abad2, hip2, knee2, foot2),\n", " ThreeLinkArm(q[9:12], num, abad3, hip3, knee3, foot3),\n", " num, support_normal_option = 'svd')\n", "\n", "\n", "\n", "# Ori task: normalized and orthogonal orientation of support area relative to base body\n", "# Below matches\n", "# R = Ry(-pi/6) @ Rx(pi/12)\n", "# Y_BodyOnSupport_Target = R[:, 1]\n", "# Z_BodyOnSupport_Target = R[:, 2]\n", "\n", "# YZ_BodyOnSupport_Target = np.array([-0.12940952, 0.96592583, 0.22414387, -0.48296291, -0.25881905, 0.8365163])\n", "# robot.set_Ori_BodyOnSupport_byYZ(YZ_BodyOnSupport_Target)\n", "\n", "# robot.set_Ori_BodyOnSupport_Target_byRPY(0, pi/6, 0)\n", "robot.set_Ori_BodyOnHorizon_Target_byRPY(0, 0, 0)\n", "\n", "\n", "\n", "\n", "# Pos task: 3d vector of middle point of foot position relative to base body\n", "robot.Pos_BodyOnHorizontalSupport_Target = np.array([0, 0, 0.26])\n", "\n", "\n", "\n", "for i in range(num):\n", " print('i: ', i)\n", " robot.it = i\n", " robot.update_joints(q)\n", " \n", " # Check distance\n", " # q_star = {q0: q[0], q1: q[1], q2: q[2], q3: q[3], q4: q[4], q5: q[5], q6: q[6], q7: q[7], q8: q[8], q9: q[9], q10: q[10], q11: q[11]}\n", " q_star = dict(zip(q_sym, q))\n", " print(s0.subs(q_star))\n", " print(s1.subs(q_star))\n", " print(s2.subs(q_star))\n", " print(s3.subs(q_star))\n", " print(s4.subs(q_star))\n", " print(s5.subs(q_star))\n", " \n", " \n", " robot.update_error()\n", " \n", "\n", " # PLOT\n", " robot.prepare_plot()\n", " robot.plot_on_body()\n", " robot.plot_on_support()\n", " robot.plot_on_horizon()\n", " \n", " # NULLSPACE CONTROLLER\n", " # Contact Task\n", " # Sustitute symbolic expression with actual joint value\n", " jc_np = np.array(jc.subs(q_star), dtype=float)\n", " \n", " # Ori Task\n", " # Calcualte Null space of contact jacobian\n", " Nc = np.eye(12) - np.linalg.pinv(jc_np, rcond=1e-11) @ jc_np\n", " # Calcualte j_pre for ori task which is in null space of contact jacobian\n", " \n", " \n", " if robot.support_normal_option == 'svd':\n", " j_ori_tail3_num = np.array(j_ori_tail3.subs(q_star), dtype = float)\n", " # NOTICE!\n", " # This is the jacobian of v5 or edgeY, NOT the jacobian of normalized v5.\n", " # However, this jacobian of v5 still ensure we will go along correct direction.\n", " dU, _, _ = autodiff_svd(A ,q_star)\n", " j_ori_head3_num = dU[:,:,-1].T\n", " j_pre = np.concatenate((j_ori_head3_num, j_ori_tail3_num)) @ Nc\n", " else:\n", " # support_normal_option = 'cross'\n", " j_pre = np.array(j_ori.subs(q_star), dtype=float) @ Nc\n", " \n", "\n", " # Pos Task\n", " # Calculate Null space of both ori and contact task.\n", " N = np.eye(12) - np.linalg.pinv(j_pre, rcond=1e-11) @ j_pre\n", " # Calculate j_pre2 for pos task which is in null space of both contact jacobian and ori\n", " j_pre2 = j_pos.subs(q_star) @ Nc @ N\n", " j_pre2 = np.array(j_pre2, dtype=float)\n", "\n", "\n", " # Calculate delta q for ori task\n", " dq = np.linalg.pinv(j_pre, rcond=1e-11) @ robot.Ori_SupportOnBody_Error\n", "\n", " # Calculate delta q for pos task\n", " dq2 = np.linalg.pinv(j_pre2, rcond=1e-11) @ robot.Pos_SupportOnBody_Error\n", " \n", " '''\n", " print(\"Ori Pre NUll space checking\")\n", " print(np.isclose(jc_np * np.linalg.pinv(j_pre, rcond=1e-9), np.zeros((1, 3))))\n", " print(\"Pos Pre Null space checking\")\n", " print(np.isclose(j_pre * np.linalg.pinv(j_pre2, rcond=1e-9), np.zeros((3, 3))))\n", " print()\n", " '''\n", " # Update q. As error decrease, make Kp become bigger and bigger to coverge\n", " # faster.\n", " \n", " # The reason why there are 2 seperated Kp: The jacobian of support\n", " # normal vector(head 3 of j_ori) achieved by svd method is for normalized\n", " # support normal vector(U[:, -1]) while the jacobian of support normal\n", " # vector (head 3 of j_ori) achieved by cross method is for raw normal\n", " # vector(cross result). So, the scale need to be adjusted. \n", " \n", " # If we wanna united 2 Kp, a possible approach might be updating 'svd'\n", " # method so that the normal jacobian(head 3 of j_ori) isn't for normalized\n", " # normal vector. It's just use U[:, -1] * S(Sigular value array, the middle\n", " # result, of standard SVD) instead of normalized U[:, -1]. \n", " \n", " # Since the svd method is more precise than other methods. If the calculating\n", " # power allow, this program should use svd method as much as possible.\n", " \n", " # Also notice, the tail 3 of j_ori isn't for normalized edgeY vector in\n", " # both 'svd' and 'cross' method. It's for raw edgeY vector.\n", " if robot.support_normal_option == 'svd':\n", " q = q + 0.9 / num * (i+1) * dq + 0.9 / num * (i+1) * dq2\n", " else:\n", " q = q + 0.2 / num * (i+1) * dq + 0.2 / num * (i+1) * dq2\n", "\n", " # q = q + dq/max(dq)*0.01 + dq2/max(dq2) *0.01\n", " # q = q + dq + dq2\n" ] }, { "cell_type": "code", "execution_count": null, "id": "681da6e5", "metadata": {}, "outputs": [], "source": [] } ], "metadata": { "colab": { "provenance": [] }, "kernelspec": { "display_name": "Python 3 (ipykernel)", "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.11.0" } }, "nbformat": 4, "nbformat_minor": 5 }