diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile new file mode 100644 index 0000000..64887d8 --- /dev/null +++ b/.devcontainer/Dockerfile @@ -0,0 +1,31 @@ +FROM ros:humble +ENV DEBIAN_FRONTEND=noninteractive +SHELL ["/bin/bash", "-c"] +RUN apt-get update && apt-get install -y -qq --no-install-recommends \ + libglvnd-dev \ + libgl1-mesa-dev \ + libegl1-mesa-dev \ + libgles2-mesa-dev \ + libxext6 \ + libx11-6 \ + freeglut3-dev \ + git \ + python3-pip \ + ros-humble-rmw-cyclonedds-cpp ros-humble-rosidl-generator-dds-idl \ + libyaml-cpp-dev \ + ros-humble-xacro \ + libboost-all-dev\ + build-essential \ + cmake \ + && rm -rf /var/lib/apt/lists/* + + +RUN pip3 install mujoco pin matplotlib + +RUN cd / && git clone https://github.com/unitreerobotics/unitree_ros2 && cd /unitree_ros2/cyclonedds_ws/src && \ +git clone https://github.com/ros2/rmw_cyclonedds -b humble && git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x &&\ +cd .. && colcon build --packages-select cyclonedds && source /opt/ros/humble/setup.bash && colcon build + +# Env vars for the nvidia-container-runtime. +ENV NVIDIA_VISIBLE_DEVICES all +ENV NVIDIA_DRIVER_CAPABILITIES graphics,utility,compute \ No newline at end of file diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 0000000..14e7378 --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,13 @@ +// https://containers.dev/implementors/json_reference/ +{ + "name": "go2py", + "dockerComposeFile": "docker-compose.yaml", + "workspaceFolder": "/home/Go2Py", + "service": "go2py-dev", + "remoteUser": "root", + "customizations": { + "vscode": { + "extensions": ["dbaeumer.vscode-eslint"] + } + } +} \ No newline at end of file diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml new file mode 100644 index 0000000..54d71e4 --- /dev/null +++ b/.devcontainer/docker-compose.yaml @@ -0,0 +1,17 @@ +version: "3.9" +services: + go2py-dev: + build: . + container_name: go2py-dev + network_mode: host + privileged: true + command: bash + volumes: + - /tmp/.X11-unix:/tmp/.X11-unix + - ..:/home/Go2Py + environment: + - DISPLAY=${DISPLAY} + - QT_X11_NO_MITSHM=1 + runtime: nvidia + stdin_open: true + tty: true \ No newline at end of file diff --git a/README.md b/README.md index ec54651..0d990ca 100644 --- a/README.md +++ b/README.md @@ -1,50 +1,68 @@ # Go2Py -Python interface, example controllers, and calibration tools for the Unitree Go2 robot. -## System Setup +Go2Py is a Pythonic interface and driver for low-level and high-level control of Unitree Go2 quadruped robots. The motivation of this project is to remove the burden of developing interface, safety systems, and basic components required for starting locomotion reserach using the Go2 quadruped robot. It provides a modular pipeline for real-time communication with the robot in both simulated and real world environment with a unified interface. -The GO2 EDU comes with an onbard Jetson Orin NX, a Hessai LX-16 LiDAR sensor, and an intel Realsense D435i camera. This setup procedure targets the onboard Jetson Orin with IP `192.168.123.18` and presents the installation proceudures of nodes for reading the sensors and robot states and publisheing them as ROS2 topics plus some basic configurations for setting up the autostart services and sharing internet. The architecture of the GO2 system is illustrated in the following image: -TODO: add image +

+ image +

-### Internet Sharing +This project is comprised of the following components: +- **C++ Bridge:** A dockerized ROS2 bridge built upon the [unitree_ros2](https://github.com/unitreerobotics/unitree_ros2) that implements a remote controlled emergency stop and publishes the robot states as standard ROS2 topics usable by upstream systems such as NAV2. +- **Robot Interface:** A simple Python class that represents the robot and communicates with the C++ bridge through either DDS (ROS independent) or ROS2 interfaces. +- **Robot Management FSM:** A finite state machine for controlling the behavior of the robot up to the point of handover to the user low-level controller (sitting down, standing up) with safety monitors (motor temperatures, emergency stops). +- **Robot Model:** A simple to use [Pinocchio](https://github.com/stack-of-tasks/pinocchio) wrapper for computing the kinematics and dynamics parameters of the robot. +- **Simulation Interface:** Simulation environments based on Mujoco and Nvidia Orbit (To be added) with a Python interface identical to the real robot. -In order to access internet on the Jetson computer, we hook the robot to a host development computer with internet access and configure it to share its connection with the robot. To cofigure the host computer, the follwoing steps should be taken: -#### Host Computer -The following steps configures the host computer to share its intentrent with the robot. -##### Enable IP forwarding: +## How Does Using it Look Like? +Communication with the robot will be as simple as importing a Python class: +```python +from Go2Py.robot.interface.dds import GO2Real +from Go2Py.robot.model import Go2Model +robot = GO2Real(mode='lowlevel') +model = Go2Model() +robot.standDownReset() +while running: + joint_state = robot.getJointStates() + imu = robot.getIMU() + remote = robot.getRemoteState() + model.update(state['q'], state['dq'],T,vel) # T and vel from the EKF + info = model.getInfo() + + #User control computations ... -```bash -sudo sysctl -w net.ipv4.ip_forward=1 + robot.setCommands(q_des, dq_des, kp, kd, tau_ff) ``` -##### Configure the iptables: +An identical workflow is can be followed for simulation: +```python +from Go2Py.sim.mujoco import Go2Sim +from Go2Py.robot.model import Go2Model +robot = Go2Sim() +model = Go2Model() +robot.standDownReset() +while running: + joint_state = robot.getJointStates() + imu = robot.getIMU() + remote = robot.getRemoteState() + model.update(state['q'], state['dq'],T,vel) # T and vel from the EKF + info = model.getInfo() -```bash -sudo iptables -t nat -A POSTROUTING -o wlan0 -j MASQUERADE -sudo iptables -A FORWARD -i wlan0 -o eth0 -m state --state RELATED,ESTABLISHED -j ACCEPT -sudo iptables -A FORWARD -i eth0 -o wlan0 -j ACCEPT + #User control computations ... + + robot.setCommands(q_des, dq_des, kp, kd, tau_ff) + robot.step() ``` -Note that `wlan0` should be replaced with the actual name of the network interface over which the internet is provided to the host computer, and eth0 should be replaced with the name of the Ethernet interface connected to the robot and having a local IP address in robot's network range. +## Installation +Follow through the steps in here to [setup](docs/setup.md) the robot and Go2Py. -##### Storing the Settings -Make the iptables rules persistent by installing the `iptables-persistent`: - -```bash -sudo apt-get install iptables-persistent -sudo iptables-save > /etc/iptables/rules.v4 -sudo ip6tables-save > /etc/iptables/rules.v6 -``` -#### Robot -Now tell the computer on the robot to use the internet shared by the host computer. SSH into the robot's computer with IP address `192.168.123.18`, username `unitree` and password `123`. Note that the host computer's IP reange should have already been set to static mode with an IP in `192.168.123.*` range. - -```bash -sudo ip route add default via -``` - -Finally, configure the DNS server by adding the following to the `/etc/resolv.conf` file: -```bash -nameserver 8.8.8.8 -``` -**Note:** Similarly to the host computer, you can make save this configuration using the `iptables-persistent` tool. - -If everything has been successful, you should be able to access the internet on the robot. Run `ping www.google.com` to verify this. +## Further Examples +A set of sorted examples are provided in the [examples](examples) directory to get you up and running quickly: +- High-level body velocity interface (ROS2) +- High-level body velocity interface (DDS) +- Low-level joint interface (ROS2) +- Low-level joint interface (DDS) +- Low-level simulation interface +- Contact Force Estimation +- Foot Contact Estimation +- Extended Kalman Filter Legged Inertial State Estimator +- Walk These Ways RL Controller diff --git a/docs/assets/openfig.drawio b/docs/assets/openfig.drawio new file mode 100644 index 0000000..c6e8355 --- /dev/null +++ b/docs/assets/openfig.drawio @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/docs/assets/openfig.png b/docs/assets/openfig.png new file mode 100644 index 0000000..8ff2c25 Binary files /dev/null and b/docs/assets/openfig.png differ diff --git a/docs/setup.md b/docs/setup.md new file mode 100644 index 0000000..942f5c3 --- /dev/null +++ b/docs/setup.md @@ -0,0 +1,47 @@ +# System Setup + +The GO2 EDU comes with an onbard Jetson Orin NX, a Hessai LX-16 LiDAR sensor, and an intel Realsense D435i camera. This setup procedure targets the onboard Jetson Orin with IP `192.168.123.18` and presents the installation proceudures of nodes for reading the sensors and robot states and publisheing them as ROS2 topics plus some basic configurations for setting up the autostart services and sharing internet. The architecture of the GO2 system is illustrated in the following image: +TODO: add image + +### Internet Sharing + +In order to access internet on the Jetson computer, we hook the robot to a host development computer with internet access and configure it to share its connection with the robot. To cofigure the host computer, the follwoing steps should be taken: +#### Host Computer +The following steps configures the host computer to share its intentrent with the robot. +##### Enable IP forwarding: + +```bash +sudo sysctl -w net.ipv4.ip_forward=1 +``` +##### Configure the iptables: + +```bash +sudo iptables -t nat -A POSTROUTING -o wlan0 -j MASQUERADE +sudo iptables -A FORWARD -i wlan0 -o eth0 -m state --state RELATED,ESTABLISHED -j ACCEPT +sudo iptables -A FORWARD -i eth0 -o wlan0 -j ACCEPT +``` +Note that `wlan0` should be replaced with the actual name of the network interface over which the internet is provided to the host computer, and eth0 should be replaced with the name of the Ethernet interface connected to the robot and having a local IP address in robot's network range. + +##### Storing the Settings +Make the iptables rules persistent by installing the `iptables-persistent`: + +```bash +sudo apt-get install iptables-persistent +sudo iptables-save > /etc/iptables/rules.v4 +sudo ip6tables-save > /etc/iptables/rules.v6 +``` +#### Robot +Now tell the computer on the robot to use the internet shared by the host computer. SSH into the robot's computer with IP address `192.168.123.18`, username `unitree` and password `123`. Note that the host computer's IP reange should have already been set to static mode with an IP in `192.168.123.*` range. + +```bash +sudo ip route add default via +``` + +Finally, configure the DNS server by adding the following to the `/etc/resolv.conf` file: +```bash +nameserver 8.8.8.8 +``` +**Note:** Similarly to the host computer, you can make save this configuration using the `iptables-persistent` tool. + +If everything has been successful, you should be able to access the internet on the robot. Run `ping www.google.com` to verify this. + diff --git a/examples/data/april16-seq1.pkl b/examples/data/april16-seq1.pkl new file mode 100644 index 0000000..e11bb86 Binary files /dev/null and b/examples/data/april16-seq1.pkl differ diff --git a/examples/data/april16-seq2.pkl b/examples/data/april16-seq2.pkl new file mode 100644 index 0000000..987b010 Binary files /dev/null and b/examples/data/april16-seq2.pkl differ diff --git a/examples/highlevel_ros2_interface.ipynb b/examples/highlevel_ros2_interface.ipynb index 0013b86..9e1bb46 100644 --- a/examples/highlevel_ros2_interface.ipynb +++ b/examples/highlevel_ros2_interface.ipynb @@ -47,7 +47,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "pygame 2.5.2 (SDL 2.28.2, Python 3.8.18)\n", + "pygame 2.5.2 (SDL 2.28.2, Python 3.8.10)\n", "Hello from the pygame community. https://www.pygame.org/contribute.html\n" ] } @@ -56,7 +56,7 @@ "from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n", "import time\n", "ros2_init()\n", - "robot = GO2Real(mode='lowlevel')\n", + "robot = GO2Real(mode='highlevel')\n", "ros2_exec_manager = ROS2ExecutorManager()\n", "ros2_exec_manager.add_node(robot)\n", "ros2_exec_manager.start()" @@ -89,24 +89,46 @@ }, { "cell_type": "code", - "execution_count": 17, + "execution_count": 50, "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "{'stamp_nanosec': 1707447716.9414463,\n", - " 'position': array([0.01655228, 0.00701126, 0.30197507]),\n", - " 'orientation': array([-0.00573182, 0.0074235 , 0.08994701, 0.99590236])}" - ] - }, - "execution_count": 17, - "metadata": {}, - "output_type": "execute_result" - } - ], + "outputs": [], "source": [ - "robot.getOdometry()" + "odometry = []\n", + "imus = []\n", + "joints = []\n", + "foot_contacts = []\n", + "stamps = []\n", + "\n", + "for i in range(60*200):\n", + " time.sleep(1/200)\n", + " odom = robot.getOdometry()\n", + " contact = robot.getFootContacts()\n", + " joint = robot.getJointStates()\n", + " imu = robot.getIMU()\n", + " imus.append(imu)\n", + " joints.append(joint)\n", + " foot_contacts.append(contact)\n", + " stamps.append(time.time())\n", + " odometry.append(odom)\n" + ] + }, + { + "cell_type": "code", + "execution_count": 51, + "metadata": {}, + "outputs": [], + "source": [ + "import pickle\n", + "with open('data/april16-seq2.pkl', 'wb') as f:\n", + " pickle.dump(\n", + " {\n", + " 'imu':imus,\n", + " 'odometry':odometry,\n", + " 'joints':joints,\n", + " 'foot_contacts':foot_contacts,\n", + " 'stamps':stamps\n", + " }, f\n", + " )" ] } ], @@ -126,7 +148,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.8.18" + "version": "3.8.10" } }, "nbformat": 4, diff --git a/examples/legged_inertial_ekf.ipynb b/examples/legged_inertial_ekf.ipynb index 8560553..8ee0dd4 100644 --- a/examples/legged_inertial_ekf.ipynb +++ b/examples/legged_inertial_ekf.ipynb @@ -1,19 +1,51 @@ { "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# EKF Derivations" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "import symforce\n", + "symforce.set_symbolic_api(\"sympy\")\n", + "symforce.set_log_level(\"warning\")\n", + "import symforce.symbolic as sf\n", + "from symforce import ops\n", + "from symforce.notebook_util import display" + ] + }, { "cell_type": "code", "execution_count": 2, "metadata": {}, "outputs": [], "source": [ - "# from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n", - "from Go2Py.robot.model import Go2Model\n", - "import time\n", - "# ros2_init()\n", - "# robot = GO2Real(mode='highlevel')\n", - "# ros2_exec_manager = ROS2ExecutorManager()\n", - "# ros2_exec_manager.add_node(robot)\n", - "# ros2_exec_manager.start()" + "# Accelerometer and gyroscope reading\n", + "gyro = sf.Vector3.symbolic('\\omega')\n", + "accel = sf.Vector3.symbolic('a')\n", + "# Accelerometer and gyroscope bias\n", + "b_a = sf.Vector3.symbolic('b_a')\n", + "b_g = sf.Vector3.symbolic('b_g')\n", + "# Gravity in world frame\n", + "gravity = sf.Vector3([0, 0, -9.8])\n", + "# Foot positions in world frame\n", + "s1 = sf.Vector3.symbolic('s_{FR}')\n", + "s2 = sf.Vector3.symbolic('s_{FL}')\n", + "s3 = sf.Vector3.symbolic('s_{RR}')\n", + "s4 = sf.Vector3.symbolic('s_{RL}')\n", + "# Integration time interval\n", + "dT = sf.Symbol('\\Delta t')\n", + "# Body orientation, position, and velocity \n", + "q = sf.Quaternion.symbolic('q')\n", + "p = sf.Vector3.symbolic('p')\n", + "v = sf.Vector3.symbolic('v')" ] }, { @@ -22,203 +54,2755 @@ "metadata": {}, "outputs": [ { - "ename": "NameError", - "evalue": "name 'robot' is not defined", - "output_type": "error", - "traceback": [ - "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[0;31mNameError\u001b[0m Traceback (most recent call last)", - "Input \u001b[0;32mIn [3]\u001b[0m, in \u001b[0;36m\u001b[0;34m()\u001b[0m\n\u001b[0;32m----> 1\u001b[0m state \u001b[38;5;241m=\u001b[39m \u001b[43mrobot\u001b[49m\u001b[38;5;241m.\u001b[39mgetJointStates()\n\u001b[1;32m 2\u001b[0m state\n", - "\u001b[0;31mNameError\u001b[0m: name 'robot' is not defined" - ] + "data": { + "text/latex": [ + "$\\displaystyle \\left[\\begin{matrix}q_{x}\\\\q_{y}\\\\q_{z}\\\\q_{w}\\\\p_{0}\\\\p_{1}\\\\p_{2}\\\\v_{0}\\\\v_{1}\\\\v_{2}\\\\b_{g0}\\\\b_{g1}\\\\b_{g2}\\\\b_{a0}\\\\b_{a1}\\\\b_{a2}\\\\s_{FR}0\\\\s_{FR}1\\\\s_{FR}2\\\\s_{FL}0\\\\s_{FL}1\\\\s_{FL}2\\\\s_{RR}0\\\\s_{RR}1\\\\s_{RR}2\\\\s_{RL}0\\\\s_{RL}1\\\\s_{RL}2\\end{matrix}\\right]$" + ], + "text/plain": [ + "⎡ qₓ ⎤\n", + "⎢ ⎥\n", + "⎢ q_y ⎥\n", + "⎢ ⎥\n", + "⎢ q_z ⎥\n", + "⎢ ⎥\n", + "⎢ q_w ⎥\n", + "⎢ ⎥\n", + "⎢ p₀ ⎥\n", + "⎢ ⎥\n", + "⎢ p₁ ⎥\n", + "⎢ ⎥\n", + "⎢ p₂ ⎥\n", + "⎢ ⎥\n", + "⎢ v₀ ⎥\n", + "⎢ ⎥\n", + "⎢ v₁ ⎥\n", + "⎢ ⎥\n", + "⎢ v₂ ⎥\n", + "⎢ ⎥\n", + "⎢ b_g0 ⎥\n", + "⎢ ⎥\n", + "⎢ b_g1 ⎥\n", + "⎢ ⎥\n", + "⎢ b_g2 ⎥\n", + "⎢ ⎥\n", + "⎢ bₐ₀ ⎥\n", + "⎢ ⎥\n", + "⎢ bₐ₁ ⎥\n", + "⎢ ⎥\n", + "⎢ bₐ₂ ⎥\n", + "⎢ ⎥\n", + "⎢s_{FR}0⎥\n", + "⎢ ⎥\n", + "⎢s_{FR}1⎥\n", + "⎢ ⎥\n", + "⎢s_{FR}2⎥\n", + "⎢ ⎥\n", + "⎢s_{FL}0⎥\n", + "⎢ ⎥\n", + "⎢s_{FL}1⎥\n", + "⎢ ⎥\n", + "⎢s_{FL}2⎥\n", + "⎢ ⎥\n", + "⎢s_{RR}0⎥\n", + "⎢ ⎥\n", + "⎢s_{RR}1⎥\n", + "⎢ ⎥\n", + "⎢s_{RR}2⎥\n", + "⎢ ⎥\n", + "⎢s_{RL}0⎥\n", + "⎢ ⎥\n", + "⎢s_{RL}1⎥\n", + "⎢ ⎥\n", + "⎣s_{RL}2⎦" + ] + }, + "metadata": {}, + "output_type": "display_data" } ], "source": [ - "state = robot.getJointStates()\n", + "state = sf.Matrix.block_matrix([[sf.Matrix([q.x, q.y, q.z, q.w])],\\\n", + " [p],\\\n", + " [v],\\\n", + " [b_g],\\\n", + " [b_a],\\\n", + " [s1],\\\n", + " [s2],\\\n", + " [s3],\\\n", + " [s4]])\n", "state" ] }, - { - "cell_type": "code", - "execution_count": 3, - "metadata": {}, - "outputs": [], - "source": [ - "import pinocchio as pin\n", - "import numpy as np" - ] - }, { "cell_type": "code", "execution_count": 4, "metadata": {}, - "outputs": [], + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\left[\\begin{matrix}\\omega0\\\\\\omega1\\\\\\omega2\\\\a_{0}\\\\a_{1}\\\\a_{2}\\end{matrix}\\right]$" + ], + "text/plain": [ + "⎡\\omega0⎤\n", + "⎢ ⎥\n", + "⎢\\omega1⎥\n", + "⎢ ⎥\n", + "⎢\\omega2⎥\n", + "⎢ ⎥\n", + "⎢ a₀ ⎥\n", + "⎢ ⎥\n", + "⎢ a₁ ⎥\n", + "⎢ ⎥\n", + "⎣ a₂ ⎦" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], "source": [ - "model = Go2Model()" + "u = sf.Matrix.block_matrix([[gyro], [accel]])\n", + "u" ] }, { "cell_type": "code", "execution_count": 5, "metadata": {}, + "outputs": [], + "source": [ + "# Process Model\n", + "f1 = q*sf.Quaternion(xyz=(gyro-b_g)*dT, w = 1)\n", + "# f1 = q*sf.Rot3.from_tangent((gyro-b_g)*dT, epsilon=epsilon).q\n", + "f1 = sf.Matrix([f1.x, f1.y, f1.z, f1.w])\n", + "f2 = p+dT*v\n", + "f3 = v+sf.Rot3(q).to_rotation_matrix()*(accel-b_a)-gravity\n", + "f4 = b_g\n", + "f5 = b_a\n", + "f6 = s1\n", + "f7 = s2\n", + "f8 = s3\n", + "f9 = s4" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [], + "source": [ + "f = sf.Matrix.block_matrix([[f1],\\\n", + " [f2],\\\n", + " [f3],\\\n", + " [f4],\\\n", + " [f5],\\\n", + " [f6],\\\n", + " [f7],\\\n", + " [f8],\\\n", + " [f9]])" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, "outputs": [ { "data": { + "text/latex": [ + "$\\displaystyle \\left[\\begin{array}{cccccccccccccccccccccccccccc}1 & \\Delta t \\left(\\omega2 - b_{g2}\\right) & - \\Delta t \\left(\\omega1 - b_{g1}\\right) & \\Delta t \\left(\\omega0 - b_{g0}\\right) & 0 & 0 & 0 & 0 & 0 & 0 & - \\Delta t q_{w} & \\Delta t q_{z} & - \\Delta t q_{y} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\- \\Delta t \\left(\\omega2 - b_{g2}\\right) & 1 & \\Delta t \\left(\\omega0 - b_{g0}\\right) & \\Delta t \\left(\\omega1 - b_{g1}\\right) & 0 & 0 & 0 & 0 & 0 & 0 & - \\Delta t q_{z} & - \\Delta t q_{w} & \\Delta t q_{x} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\\\Delta t \\left(\\omega1 - b_{g1}\\right) & - \\Delta t \\left(\\omega0 - b_{g0}\\right) & 1 & \\Delta t \\left(\\omega2 - b_{g2}\\right) & 0 & 0 & 0 & 0 & 0 & 0 & \\Delta t q_{y} & - \\Delta t q_{x} & - \\Delta t q_{w} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\- \\Delta t \\left(\\omega0 - b_{g0}\\right) & - \\Delta t \\left(\\omega1 - b_{g1}\\right) & - \\Delta t \\left(\\omega2 - b_{g2}\\right) & 1 & 0 & 0 & 0 & 0 & 0 & 0 & \\Delta t q_{x} & \\Delta t q_{y} & \\Delta t q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 1 & 0 & 0 & \\Delta t & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & \\Delta t & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & \\Delta t & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\2 q_{y} \\left(a_{1} - b_{a1}\\right) + 2 q_{z} \\left(a_{2} - b_{a2}\\right) & 2 q_{w} \\left(a_{2} - b_{a2}\\right) + 2 q_{x} \\left(a_{1} - b_{a1}\\right) - 4 q_{y} \\left(a_{0} - b_{a0}\\right) & - 2 q_{w} \\left(a_{1} - b_{a1}\\right) + 2 q_{x} \\left(a_{2} - b_{a2}\\right) - 4 q_{z} \\left(a_{0} - b_{a0}\\right) & 2 q_{y} \\left(a_{2} - b_{a2}\\right) - 2 q_{z} \\left(a_{1} - b_{a1}\\right) & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 2 q_{y}^{2} + 2 q_{z}^{2} - 1 & 2 q_{w} q_{z} - 2 q_{x} q_{y} & - 2 q_{w} q_{y} - 2 q_{x} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\- 2 q_{w} \\left(a_{2} - b_{a2}\\right) - 4 q_{x} \\left(a_{1} - b_{a1}\\right) + 2 q_{y} \\left(a_{0} - b_{a0}\\right) & 2 q_{x} \\left(a_{0} - b_{a0}\\right) + 2 q_{z} \\left(a_{2} - b_{a2}\\right) & 2 q_{w} \\left(a_{0} - b_{a0}\\right) + 2 q_{y} \\left(a_{2} - b_{a2}\\right) - 4 q_{z} \\left(a_{1} - b_{a1}\\right) & - 2 q_{x} \\left(a_{2} - b_{a2}\\right) + 2 q_{z} \\left(a_{0} - b_{a0}\\right) & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & - 2 q_{w} q_{z} - 2 q_{x} q_{y} & 2 q_{x}^{2} + 2 q_{z}^{2} - 1 & 2 q_{w} q_{x} - 2 q_{y} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\2 q_{w} \\left(a_{1} - b_{a1}\\right) - 4 q_{x} \\left(a_{2} - b_{a2}\\right) + 2 q_{z} \\left(a_{0} - b_{a0}\\right) & - 2 q_{w} \\left(a_{0} - b_{a0}\\right) - 4 q_{y} \\left(a_{2} - b_{a2}\\right) + 2 q_{z} \\left(a_{1} - b_{a1}\\right) & 2 q_{x} \\left(a_{0} - b_{a0}\\right) + 2 q_{y} \\left(a_{1} - b_{a1}\\right) & 2 q_{x} \\left(a_{1} - b_{a1}\\right) - 2 q_{y} \\left(a_{0} - b_{a0}\\right) & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 2 q_{w} q_{y} - 2 q_{x} q_{z} & - 2 q_{w} q_{x} - 2 q_{y} q_{z} & 2 q_{x}^{2} + 2 q_{y}^{2} - 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\\end{array}\\right]$" + ], "text/plain": [ - "{'FR_foot': array([[ 2.54449360e-02, 4.33674972e-02, 9.98735108e-01,\n", - " -1.41910262e-01, -2.05117030e-01, 1.25221489e-02,\n", - " -9.54690795e-02, -1.20784475e-02, -2.13000000e-01,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", - " [ 0.00000000e+00, 9.99058578e-01, -4.33815430e-02,\n", - " 7.30090366e-02, 8.83575282e-03, 2.03483648e-01,\n", - " 7.09917949e-02, 7.04731412e-19, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", - " [-9.99676225e-01, 1.10384059e-03, 2.54209816e-02,\n", - " -3.61206704e-03, 6.15827735e-02, -1.44718059e-01,\n", - " -2.42999139e-03, 7.07073528e-02, -1.04083409e-17,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", - " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 2.54449360e-02, 4.33674972e-02, 9.98735108e-01,\n", - " 2.54449360e-02, -6.93889390e-18, -6.93889390e-18,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", - " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 9.99058578e-01, -4.33815430e-02,\n", - " 0.00000000e+00, 1.00000000e+00, 1.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", - " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " -9.99676225e-01, 1.10384059e-03, 2.54209816e-02,\n", - " -9.99676225e-01, -4.33680869e-19, -4.33680869e-19,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]]),\n", - " 'FL_foot': array([[ 4.69705590e-02, -1.64291856e-02, 9.98761157e-01,\n", - " 1.41836988e-01, -2.06048603e-01, -1.00598359e-02,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 9.53945942e-02, -1.29637269e-02, -2.13000000e-01,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", - " [ 0.00000000e+00, 9.99864733e-01, 1.64473389e-02,\n", - " 7.44673272e-02, -3.33736863e-03, 2.02884930e-01,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 7.37025259e-02, 5.42101086e-20, 4.33680869e-19,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", - " [-9.98896274e-01, -7.72540702e-04, 4.69642054e-02,\n", - " 6.66952394e-03, 6.25126142e-02, 1.42884482e-01,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 4.48568838e-03, 7.31743768e-02, 5.20417043e-18,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", - " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 4.69705590e-02, -1.64291856e-02, 9.98761157e-01,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 4.69705590e-02, 3.46944695e-18, 3.46944695e-18,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", - " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 9.99864733e-01, 1.64473389e-02,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 1.00000000e+00, 1.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", - " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " -9.98896274e-01, -7.72540702e-04, 4.69642054e-02,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " -9.98896274e-01, 1.08420217e-19, 1.08420217e-19,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]]),\n", - " 'RR_foot': array([[ 4.05169215e-02, 3.55928916e-01, 9.33634289e-01,\n", - " -1.38835575e-01, 1.70692580e-01, -5.90479974e-02,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " -9.54215804e-02, -1.20404787e-02, -2.13000000e-01,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", - " [ 0.00000000e+00, 9.34401571e-01, -3.56221426e-01,\n", - " 8.75935353e-02, -6.56266297e-02, -1.72144687e-01,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 7.10292389e-02, -4.33680869e-19, 2.77555756e-17,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", - " [-9.99178852e-01, 1.44329956e-02, 3.78590751e-02,\n", - " -5.62981300e-03, 3.92988943e-02, -1.63564240e-01,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " -3.86936601e-03, 7.05993683e-02, -1.56125113e-17,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", - " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 4.05169215e-02, 3.55928916e-01, 9.33634289e-01,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 4.05169215e-02, -5.55111512e-17, -5.55111512e-17,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", - " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 9.34401571e-01, -3.56221426e-01,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 1.00000000e+00, 1.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", - " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " -9.99178852e-01, 1.44329956e-02, 3.78590751e-02,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " -9.99178852e-01, 2.60208521e-17, 2.60208521e-17,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]]),\n", - " 'RL_foot': array([[ 4.02773873e-02, -3.34042417e-01, 9.41697083e-01,\n", - " 1.39211420e-01, 1.72418133e-01, 5.52066037e-02,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 9.54225053e-02, -1.16630895e-02, -2.13000000e-01],\n", - " [ 0.00000000e+00, 9.42461856e-01, 3.34313700e-01,\n", - " 8.54747509e-02, 6.16963531e-02, -1.73927839e-01,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 6.99291638e-02, 1.12757026e-17, 0.00000000e+00],\n", - " [-9.99188537e-01, -1.34652824e-02, 3.79599012e-02,\n", - " 5.61162590e-03, 4.09563977e-02, 1.62238613e-01,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 3.84649049e-03, 6.95158146e-02, -6.93889390e-18],\n", - " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 4.02773873e-02, -3.34042417e-01, 9.41697083e-01,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 4.02773873e-02, -5.55111512e-17, -5.55111512e-17],\n", - " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 9.42461856e-01, 3.34313700e-01,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 1.00000000e+00, 1.00000000e+00],\n", - " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " -9.99188537e-01, -1.34652824e-02, 3.79599012e-02,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " -9.99188537e-01, -5.20417043e-18, -5.20417043e-18]])}" + "⎡ 1 \\Delta\n", + "⎢ \n", + "⎢ -\\Delta t⋅(\\omega2 - b_g2) \n", + "⎢ \n", + "⎢ \\Delta t⋅(\\omega1 - b_g1) -\\Delta\n", + "⎢ \n", + "⎢ -\\Delta t⋅(\\omega0 - b_g0) -\\Delta\n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎢ \n", + "⎢ 2⋅q_y⋅(a₁ - bₐ₁) + 2⋅q_z⋅(a₂ - bₐ₂) 2⋅q_w⋅(a₂ - bₐ₂) + 2\n", + "⎢ \n", + "⎢ \n", + "⎢-2⋅q_w⋅(a₂ - bₐ₂) - 4⋅qₓ⋅(a₁ - bₐ₁) + 2⋅q_y⋅(a₀ - bₐ₀) 2⋅qₓ⋅(a₀ - \n", + "⎢ \n", + "⎢ \n", + "⎢2⋅q_w⋅(a₁ - bₐ₁) - 4⋅qₓ⋅(a₂ - bₐ₂) + 2⋅q_z⋅(a₀ - bₐ₀) -2⋅q_w⋅(a₀ - bₐ₀) - 4\n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎢ 0 \n", + "⎢ \n", + "⎣ 0 \n", + "\n", + " t⋅(\\omega2 - b_g2) -\\Delta t⋅(\\omega1 - b_g1) \n", + " \n", + " 1 \\Delta t⋅(\\omega0 - b_g0) \n", + " \n", + " t⋅(\\omega0 - b_g0) 1 \n", + " \n", + " t⋅(\\omega1 - b_g1) -\\Delta t⋅(\\omega2 - b_g2) \n", + " \n", + " 0 0 \n", + " \n", + " 0 0 \n", + " \n", + " 0 0 \n", + " \n", + " \n", + "⋅qₓ⋅(a₁ - bₐ₁) - 4⋅q_y⋅(a₀ - bₐ₀) -2⋅q_w⋅(a₁ - bₐ₁) + 2⋅qₓ⋅(a₂ - bₐ₂) - 4⋅q_\n", + " \n", + " \n", + "bₐ₀) + 2⋅q_z⋅(a₂ - bₐ₂) 2⋅q_w⋅(a₀ - bₐ₀) + 2⋅q_y⋅(a₂ - bₐ₂) - 4⋅q_\n", + " \n", + " \n", + "⋅q_y⋅(a₂ - bₐ₂) + 2⋅q_z⋅(a₁ - bₐ₁) 2⋅qₓ⋅(a₀ - bₐ₀) + 2⋅q_y⋅(a₁ - bₐ\n", + " \n", + " 0 0 \n", + " \n", + " 0 0 \n", + " \n", + " 0 0 \n", + " \n", + " 0 0 \n", + " \n", + " 0 0 \n", + " \n", + " 0 0 \n", + " \n", + " 0 0 \n", + " \n", + " 0 0 \n", + " \n", + " 0 0 \n", + " \n", + " 0 0 \n", + " \n", + " 0 0 \n", + " \n", + " 0 0 \n", + " \n", + " 0 0 \n", + " \n", + " 0 0 \n", + " \n", + " 0 0 \n", + " \n", + " 0 0 \n", + " \n", + " 0 0 \n", + " \n", + " 0 0 \n", + "\n", + " \\Delta t⋅(\\omega0 - b_g0) 0 0 0 0 0 \n", + " \n", + " \\Delta t⋅(\\omega1 - b_g1) 0 0 0 0 0 \n", + " \n", + " \\Delta t⋅(\\omega2 - b_g2) 0 0 0 0 0 \n", + " \n", + " 1 0 0 0 0 0 \n", + " \n", + " 0 1 0 0 \\Delta t 0 \n", + " \n", + " 0 0 1 0 0 \\Delta t\n", + " \n", + " 0 0 0 1 0 0 \n", + " \n", + " \n", + "z⋅(a₀ - bₐ₀) 2⋅q_y⋅(a₂ - bₐ₂) - 2⋅q_z⋅(a₁ - bₐ₁) 0 0 0 1 0 \n", + " \n", + " \n", + "z⋅(a₁ - bₐ₁) -2⋅qₓ⋅(a₂ - bₐ₂) + 2⋅q_z⋅(a₀ - bₐ₀) 0 0 0 0 1 \n", + " \n", + " \n", + "₁) 2⋅qₓ⋅(a₁ - bₐ₁) - 2⋅q_y⋅(a₀ - bₐ₀) 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + "\n", + " 0 -\\Delta t⋅q_w \\Delta t⋅q_z -\\Delta t⋅q_y 0 \n", + " \n", + " 0 -\\Delta t⋅q_z -\\Delta t⋅q_w \\Delta t⋅qₓ 0 \n", + " \n", + " 0 \\Delta t⋅q_y -\\Delta t⋅qₓ -\\Delta t⋅q_w 0 \n", + " \n", + " 0 \\Delta t⋅qₓ \\Delta t⋅q_y \\Delta t⋅q_z 0 \n", + " \n", + " 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 \n", + " \n", + " \\Delta t 0 0 0 0 \n", + " \n", + " 2 2 \n", + " 0 0 0 0 2⋅q_y + 2⋅q_z - 1 \n", + " \n", + " \n", + " 0 0 0 0 -2⋅q_w⋅q_z - 2⋅qₓ⋅q_y\n", + " \n", + " \n", + " 1 0 0 0 2⋅q_w⋅q_y - 2⋅qₓ⋅q_z \n", + " \n", + " 0 1 0 0 0 \n", + " \n", + " 0 0 1 0 0 \n", + " \n", + " 0 0 0 1 0 \n", + " \n", + " 0 0 0 0 1 \n", + " \n", + " 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 \n", + "\n", + " 0 0 0 0 0 0 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 0 0 0 0 0 0 \n", + " \n", + " \n", + " 2⋅q_w⋅q_z - 2⋅qₓ⋅q_y -2⋅q_w⋅q_y - 2⋅qₓ⋅q_z 0 0 0 0 0 0 0 0 0 0 \n", + " \n", + " 2 2 \n", + " 2⋅qₓ + 2⋅q_z - 1 2⋅q_w⋅qₓ - 2⋅q_y⋅q_z 0 0 0 0 0 0 0 0 0 0 \n", + " \n", + " 2 2 \n", + " -2⋅q_w⋅qₓ - 2⋅q_y⋅q_z 2⋅qₓ + 2⋅q_y - 1 0 0 0 0 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 0 0 0 0 0 0 \n", + " \n", + " 1 0 0 0 0 0 0 0 0 0 0 0 \n", + " \n", + " 0 1 0 0 0 0 0 0 0 0 0 0 \n", + " \n", + " 0 0 1 0 0 0 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 1 0 0 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 1 0 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 1 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 1 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 0 1 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 0 0 1 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 0 0 0 1 0 0 \n", + " \n", + " 0 0 0 0 0 0 0 0 0 0 1 0 \n", + " \n", + " 0 0 0 0 0 0 0 0 0 0 0 1 \n", + " \n", + " 0 0 0 0 0 0 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 0 0 0 0 0 0 \n", + "\n", + "0 0⎤\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "0 0⎥\n", + " ⎥\n", + "1 0⎥\n", + " ⎥\n", + "0 1⎦" ] }, - "execution_count": 5, "metadata": {}, - "output_type": "execute_result" + "output_type": "display_data" } ], "source": [ - "model.updateKinematicsPose(state['q'], np.eye(4))\n", - "model.getInfo()['Jb']" + "A_x = f.jacobian(state)\n", + "A_x " + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\left[\\begin{matrix}\\Delta t q_{w} & - \\Delta t q_{z} & \\Delta t q_{y} & 0 & 0 & 0\\\\\\Delta t q_{z} & \\Delta t q_{w} & - \\Delta t q_{x} & 0 & 0 & 0\\\\- \\Delta t q_{y} & \\Delta t q_{x} & \\Delta t q_{w} & 0 & 0 & 0\\\\- \\Delta t q_{x} & - \\Delta t q_{y} & - \\Delta t q_{z} & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & - 2 q_{y}^{2} - 2 q_{z}^{2} + 1 & - 2 q_{w} q_{z} + 2 q_{x} q_{y} & 2 q_{w} q_{y} + 2 q_{x} q_{z}\\\\0 & 0 & 0 & 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{x}^{2} - 2 q_{z}^{2} + 1 & - 2 q_{w} q_{x} + 2 q_{y} q_{z}\\\\0 & 0 & 0 & - 2 q_{w} q_{y} + 2 q_{x} q_{z} & 2 q_{w} q_{x} + 2 q_{y} q_{z} & - 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\end{matrix}\\right]$" + ], + "text/plain": [ + "⎡\\Delta t⋅q_w -\\Delta t⋅q_z \\Delta t⋅q_y 0 \n", + "⎢ \n", + "⎢\\Delta t⋅q_z \\Delta t⋅q_w -\\Delta t⋅qₓ 0 \n", + "⎢ \n", + "⎢-\\Delta t⋅q_y \\Delta t⋅qₓ \\Delta t⋅q_w 0 \n", + "⎢ \n", + "⎢-\\Delta t⋅qₓ -\\Delta t⋅q_y -\\Delta t⋅q_z 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎢ 2 2 \n", + "⎢ 0 0 0 - 2⋅q_y - 2⋅q_z + 1 -2⋅q_w⋅q_\n", + "⎢ \n", + "⎢ 2 \n", + "⎢ 0 0 0 2⋅q_w⋅q_z + 2⋅qₓ⋅q_y - 2⋅qₓ -\n", + "⎢ \n", + "⎢ \n", + "⎢ 0 0 0 -2⋅q_w⋅q_y + 2⋅qₓ⋅q_z 2⋅q_w⋅qₓ \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 \n", + "⎢ \n", + "⎣ 0 0 0 0 \n", + "\n", + " 0 0 ⎤\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " ⎥\n", + "z + 2⋅qₓ⋅q_y 2⋅q_w⋅q_y + 2⋅qₓ⋅q_z ⎥\n", + " ⎥\n", + " 2 ⎥\n", + " 2⋅q_z + 1 -2⋅q_w⋅qₓ + 2⋅q_y⋅q_z⎥\n", + " ⎥\n", + " 2 2 ⎥\n", + "+ 2⋅q_y⋅q_z - 2⋅qₓ - 2⋅q_y + 1 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎥\n", + " ⎥\n", + " 0 0 ⎦" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "A_u = f.jacobian(u)\n", + "A_u" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\left[\\begin{matrix}v_{0} \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + v_{1} \\cdot \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + v_{2} \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\v_{0} \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + v_{1} \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + v_{2} \\cdot \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\v_{0} \\cdot \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + v_{1} \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + v_{2} \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\\\\\left(- p_{0} + s_{FR}0\\right) \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{1} + s_{FR}1\\right) \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{2} + s_{FR}2\\right) \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\\\left(- p_{0} + s_{FR}0\\right) \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{1} + s_{FR}1\\right) \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{2} + s_{FR}2\\right) \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\\\left(- p_{0} + s_{FR}0\\right) \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + \\left(- p_{1} + s_{FR}1\\right) \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + \\left(- p_{2} + s_{FR}2\\right) \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\\\\\left(- p_{0} + s_{FL}0\\right) \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{1} + s_{FL}1\\right) \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{2} + s_{FL}2\\right) \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\\\left(- p_{0} + s_{FL}0\\right) \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{1} + s_{FL}1\\right) \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{2} + s_{FL}2\\right) \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\\\left(- p_{0} + s_{FL}0\\right) \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + \\left(- p_{1} + s_{FL}1\\right) \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + \\left(- p_{2} + s_{FL}2\\right) \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\\\\\left(- p_{0} + s_{RR}0\\right) \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{1} + s_{RR}1\\right) \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{2} + s_{RR}2\\right) \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\\\left(- p_{0} + s_{RR}0\\right) \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{1} + s_{RR}1\\right) \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{2} + s_{RR}2\\right) \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\\\left(- p_{0} + s_{RR}0\\right) \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + \\left(- p_{1} + s_{RR}1\\right) \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + \\left(- p_{2} + s_{RR}2\\right) \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\\\\\left(- p_{0} + s_{RL}0\\right) \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{1} + s_{RL}1\\right) \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{2} + s_{RL}2\\right) \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\\\left(- p_{0} + s_{RL}0\\right) \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{1} + s_{RL}1\\right) \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{2} + s_{RL}2\\right) \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\\\left(- p_{0} + s_{RL}0\\right) \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + \\left(- p_{1} + s_{RL}1\\right) \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + \\left(- p_{2} + s_{RL}2\\right) \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\end{matrix}\\right]$" + ], + "text/plain": [ + "⎡ ⎛ 2 2 ⎞ \n", + "⎢ v₀⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + v₁⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + v\n", + "⎢ \n", + "⎢ ⎛ 2 2 ⎞ \n", + "⎢ v₀⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + v₁⋅⎝- 2⋅qₓ - 2⋅q_z + 1⎠ + \n", + "⎢ \n", + "⎢ \n", + "⎢ v₀⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + v₁⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) + \n", + "⎢ \n", + "⎢ ⎛ 2 2 ⎞ \n", + "⎢(-p₀ + s_{FR}0)⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + (-p₁ + s_{FR}1)⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q\n", + "⎢ \n", + "⎢ ⎛ 2 2 \n", + "⎢(-p₀ + s_{FR}0)⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + (-p₁ + s_{FR}1)⋅⎝- 2⋅qₓ - 2⋅q_z +\n", + "⎢ \n", + "⎢ \n", + "⎢(-p₀ + s_{FR}0)⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + (-p₁ + s_{FR}1)⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q\n", + "⎢ \n", + "⎢ ⎛ 2 2 ⎞ \n", + "⎢(-p₀ + s_{FL}0)⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + (-p₁ + s_{FL}1)⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q\n", + "⎢ \n", + "⎢ ⎛ 2 2 \n", + "⎢(-p₀ + s_{FL}0)⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + (-p₁ + s_{FL}1)⋅⎝- 2⋅qₓ - 2⋅q_z +\n", + "⎢ \n", + "⎢ \n", + "⎢(-p₀ + s_{FL}0)⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + (-p₁ + s_{FL}1)⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q\n", + "⎢ \n", + "⎢ ⎛ 2 2 ⎞ \n", + "⎢(-p₀ + s_{RR}0)⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + (-p₁ + s_{RR}1)⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q\n", + "⎢ \n", + "⎢ ⎛ 2 2 \n", + "⎢(-p₀ + s_{RR}0)⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + (-p₁ + s_{RR}1)⋅⎝- 2⋅qₓ - 2⋅q_z +\n", + "⎢ \n", + "⎢ \n", + "⎢(-p₀ + s_{RR}0)⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + (-p₁ + s_{RR}1)⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q\n", + "⎢ \n", + "⎢ ⎛ 2 2 ⎞ \n", + "⎢(-p₀ + s_{RL}0)⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + (-p₁ + s_{RL}1)⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q\n", + "⎢ \n", + "⎢ ⎛ 2 2 \n", + "⎢(-p₀ + s_{RL}0)⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + (-p₁ + s_{RL}1)⋅⎝- 2⋅qₓ - 2⋅q_z +\n", + "⎢ \n", + "⎢ \n", + "⎣(-p₀ + s_{RL}0)⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + (-p₁ + s_{RL}1)⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q\n", + "\n", + " ⎤\n", + "₂⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) ⎥\n", + " ⎥\n", + " ⎥\n", + "v₂⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n", + " ⎥\n", + " ⎛ 2 2 ⎞ ⎥\n", + "v₂⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎥\n", + " ⎥\n", + " ⎥\n", + "_y) + (-p₂ + s_{FR}2)⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z)⎥\n", + " ⎥\n", + " ⎞ ⎥\n", + " 1⎠ + (-p₂ + s_{FR}2)⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n", + " ⎥\n", + " ⎛ 2 2 ⎞ ⎥\n", + "_z) + (-p₂ + s_{FR}2)⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎥\n", + " ⎥\n", + " ⎥\n", + "_y) + (-p₂ + s_{FL}2)⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z)⎥\n", + " ⎥\n", + " ⎞ ⎥\n", + " 1⎠ + (-p₂ + s_{FL}2)⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n", + " ⎥\n", + " ⎛ 2 2 ⎞ ⎥\n", + "_z) + (-p₂ + s_{FL}2)⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎥\n", + " ⎥\n", + " ⎥\n", + "_y) + (-p₂ + s_{RR}2)⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z)⎥\n", + " ⎥\n", + " ⎞ ⎥\n", + " 1⎠ + (-p₂ + s_{RR}2)⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n", + " ⎥\n", + " ⎛ 2 2 ⎞ ⎥\n", + "_z) + (-p₂ + s_{RR}2)⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎥\n", + " ⎥\n", + " ⎥\n", + "_y) + (-p₂ + s_{RL}2)⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z)⎥\n", + " ⎥\n", + " ⎞ ⎥\n", + " 1⎠ + (-p₂ + s_{RL}2)⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n", + " ⎥\n", + " ⎛ 2 2 ⎞ ⎥\n", + "_z) + (-p₂ + s_{RL}2)⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎦" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "#Measurement model\n", + "R = sf.Rot3(q).to_rotation_matrix()\n", + "z1 = R.T*v\n", + "z2 = R.T*(s1-p)\n", + "z3 = R.T*(s2-p)\n", + "z4 = R.T*(s3-p)\n", + "z5 = R.T*(s4-p)\n", + "h = sf.Matrix.block_matrix([[z1],\n", + " [z2],\n", + " [z3],\n", + " [z4],\n", + " [z5]])\n", + "h" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\left[\\begin{array}{cccccccccccccccccccccccccccc}2 q_{y} v_{1} + 2 q_{z} v_{2} & - 2 q_{w} v_{2} + 2 q_{x} v_{1} - 4 q_{y} v_{0} & 2 q_{w} v_{1} + 2 q_{x} v_{2} - 4 q_{z} v_{0} & - 2 q_{y} v_{2} + 2 q_{z} v_{1} & 0 & 0 & 0 & - 2 q_{y}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{w} q_{y} + 2 q_{x} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\2 q_{w} v_{2} - 4 q_{x} v_{1} + 2 q_{y} v_{0} & 2 q_{x} v_{0} + 2 q_{z} v_{2} & - 2 q_{w} v_{0} + 2 q_{y} v_{2} - 4 q_{z} v_{1} & 2 q_{x} v_{2} - 2 q_{z} v_{0} & 0 & 0 & 0 & - 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{x}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{x} + 2 q_{y} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\- 2 q_{w} v_{1} - 4 q_{x} v_{2} + 2 q_{z} v_{0} & 2 q_{w} v_{0} - 4 q_{y} v_{2} + 2 q_{z} v_{1} & 2 q_{x} v_{0} + 2 q_{y} v_{1} & - 2 q_{x} v_{1} + 2 q_{y} v_{0} & 0 & 0 & 0 & 2 q_{w} q_{y} + 2 q_{x} q_{z} & - 2 q_{w} q_{x} + 2 q_{y} q_{z} & - 2 q_{x}^{2} - 2 q_{y}^{2} + 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\2 q_{y} \\left(- p_{1} + s_{FR}1\\right) + 2 q_{z} \\left(- p_{2} + s_{FR}2\\right) & - 2 q_{w} \\left(- p_{2} + s_{FR}2\\right) + 2 q_{x} \\left(- p_{1} + s_{FR}1\\right) - 4 q_{y} \\left(- p_{0} + s_{FR}0\\right) & 2 q_{w} \\left(- p_{1} + s_{FR}1\\right) + 2 q_{x} \\left(- p_{2} + s_{FR}2\\right) - 4 q_{z} \\left(- p_{0} + s_{FR}0\\right) & - 2 q_{y} \\left(- p_{2} + s_{FR}2\\right) + 2 q_{z} \\left(- p_{1} + s_{FR}1\\right) & 2 q_{y}^{2} + 2 q_{z}^{2} - 1 & - 2 q_{w} q_{z} - 2 q_{x} q_{y} & 2 q_{w} q_{y} - 2 q_{x} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 2 q_{y}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{w} q_{y} + 2 q_{x} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\2 q_{w} \\left(- p_{2} + s_{FR}2\\right) - 4 q_{x} \\left(- p_{1} + s_{FR}1\\right) + 2 q_{y} \\left(- p_{0} + s_{FR}0\\right) & 2 q_{x} \\left(- p_{0} + s_{FR}0\\right) + 2 q_{z} \\left(- p_{2} + s_{FR}2\\right) & - 2 q_{w} \\left(- p_{0} + s_{FR}0\\right) + 2 q_{y} \\left(- p_{2} + s_{FR}2\\right) - 4 q_{z} \\left(- p_{1} + s_{FR}1\\right) & 2 q_{x} \\left(- p_{2} + s_{FR}2\\right) - 2 q_{z} \\left(- p_{0} + s_{FR}0\\right) & 2 q_{w} q_{z} - 2 q_{x} q_{y} & 2 q_{x}^{2} + 2 q_{z}^{2} - 1 & - 2 q_{w} q_{x} - 2 q_{y} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{x}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{x} + 2 q_{y} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\- 2 q_{w} \\left(- p_{1} + s_{FR}1\\right) - 4 q_{x} \\left(- p_{2} + s_{FR}2\\right) + 2 q_{z} \\left(- p_{0} + s_{FR}0\\right) & 2 q_{w} \\left(- p_{0} + s_{FR}0\\right) - 4 q_{y} \\left(- p_{2} + s_{FR}2\\right) + 2 q_{z} \\left(- p_{1} + s_{FR}1\\right) & 2 q_{x} \\left(- p_{0} + s_{FR}0\\right) + 2 q_{y} \\left(- p_{1} + s_{FR}1\\right) & - 2 q_{x} \\left(- p_{1} + s_{FR}1\\right) + 2 q_{y} \\left(- p_{0} + s_{FR}0\\right) & - 2 q_{w} q_{y} - 2 q_{x} q_{z} & 2 q_{w} q_{x} - 2 q_{y} q_{z} & 2 q_{x}^{2} + 2 q_{y}^{2} - 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 2 q_{w} q_{y} + 2 q_{x} q_{z} & - 2 q_{w} q_{x} + 2 q_{y} q_{z} & - 2 q_{x}^{2} - 2 q_{y}^{2} + 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\2 q_{y} \\left(- p_{1} + s_{FL}1\\right) + 2 q_{z} \\left(- p_{2} + s_{FL}2\\right) & - 2 q_{w} \\left(- p_{2} + s_{FL}2\\right) + 2 q_{x} \\left(- p_{1} + s_{FL}1\\right) - 4 q_{y} \\left(- p_{0} + s_{FL}0\\right) & 2 q_{w} \\left(- p_{1} + s_{FL}1\\right) + 2 q_{x} \\left(- p_{2} + s_{FL}2\\right) - 4 q_{z} \\left(- p_{0} + s_{FL}0\\right) & - 2 q_{y} \\left(- p_{2} + s_{FL}2\\right) + 2 q_{z} \\left(- p_{1} + s_{FL}1\\right) & 2 q_{y}^{2} + 2 q_{z}^{2} - 1 & - 2 q_{w} q_{z} - 2 q_{x} q_{y} & 2 q_{w} q_{y} - 2 q_{x} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 2 q_{y}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{w} q_{y} + 2 q_{x} q_{z} & 0 & 0 & 0 & 0 & 0 & 0\\\\2 q_{w} \\left(- p_{2} + s_{FL}2\\right) - 4 q_{x} \\left(- p_{1} + s_{FL}1\\right) + 2 q_{y} \\left(- p_{0} + s_{FL}0\\right) & 2 q_{x} \\left(- p_{0} + s_{FL}0\\right) + 2 q_{z} \\left(- p_{2} + s_{FL}2\\right) & - 2 q_{w} \\left(- p_{0} + s_{FL}0\\right) + 2 q_{y} \\left(- p_{2} + s_{FL}2\\right) - 4 q_{z} \\left(- p_{1} + s_{FL}1\\right) & 2 q_{x} \\left(- p_{2} + s_{FL}2\\right) - 2 q_{z} \\left(- p_{0} + s_{FL}0\\right) & 2 q_{w} q_{z} - 2 q_{x} q_{y} & 2 q_{x}^{2} + 2 q_{z}^{2} - 1 & - 2 q_{w} q_{x} - 2 q_{y} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{x}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{x} + 2 q_{y} q_{z} & 0 & 0 & 0 & 0 & 0 & 0\\\\- 2 q_{w} \\left(- p_{1} + s_{FL}1\\right) - 4 q_{x} \\left(- p_{2} + s_{FL}2\\right) + 2 q_{z} \\left(- p_{0} + s_{FL}0\\right) & 2 q_{w} \\left(- p_{0} + s_{FL}0\\right) - 4 q_{y} \\left(- p_{2} + s_{FL}2\\right) + 2 q_{z} \\left(- p_{1} + s_{FL}1\\right) & 2 q_{x} \\left(- p_{0} + s_{FL}0\\right) + 2 q_{y} \\left(- p_{1} + s_{FL}1\\right) & - 2 q_{x} \\left(- p_{1} + s_{FL}1\\right) + 2 q_{y} \\left(- p_{0} + s_{FL}0\\right) & - 2 q_{w} q_{y} - 2 q_{x} q_{z} & 2 q_{w} q_{x} - 2 q_{y} q_{z} & 2 q_{x}^{2} + 2 q_{y}^{2} - 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 2 q_{w} q_{y} + 2 q_{x} q_{z} & - 2 q_{w} q_{x} + 2 q_{y} q_{z} & - 2 q_{x}^{2} - 2 q_{y}^{2} + 1 & 0 & 0 & 0 & 0 & 0 & 0\\\\2 q_{y} \\left(- p_{1} + s_{RR}1\\right) + 2 q_{z} \\left(- p_{2} + s_{RR}2\\right) & - 2 q_{w} \\left(- p_{2} + s_{RR}2\\right) + 2 q_{x} \\left(- p_{1} + s_{RR}1\\right) - 4 q_{y} \\left(- p_{0} + s_{RR}0\\right) & 2 q_{w} \\left(- p_{1} + s_{RR}1\\right) + 2 q_{x} \\left(- p_{2} + s_{RR}2\\right) - 4 q_{z} \\left(- p_{0} + s_{RR}0\\right) & - 2 q_{y} \\left(- p_{2} + s_{RR}2\\right) + 2 q_{z} \\left(- p_{1} + s_{RR}1\\right) & 2 q_{y}^{2} + 2 q_{z}^{2} - 1 & - 2 q_{w} q_{z} - 2 q_{x} q_{y} & 2 q_{w} q_{y} - 2 q_{x} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 2 q_{y}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{w} q_{y} + 2 q_{x} q_{z} & 0 & 0 & 0\\\\2 q_{w} \\left(- p_{2} + s_{RR}2\\right) - 4 q_{x} \\left(- p_{1} + s_{RR}1\\right) + 2 q_{y} \\left(- p_{0} + s_{RR}0\\right) & 2 q_{x} \\left(- p_{0} + s_{RR}0\\right) + 2 q_{z} \\left(- p_{2} + s_{RR}2\\right) & - 2 q_{w} \\left(- p_{0} + s_{RR}0\\right) + 2 q_{y} \\left(- p_{2} + s_{RR}2\\right) - 4 q_{z} \\left(- p_{1} + s_{RR}1\\right) & 2 q_{x} \\left(- p_{2} + s_{RR}2\\right) - 2 q_{z} \\left(- p_{0} + s_{RR}0\\right) & 2 q_{w} q_{z} - 2 q_{x} q_{y} & 2 q_{x}^{2} + 2 q_{z}^{2} - 1 & - 2 q_{w} q_{x} - 2 q_{y} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{x}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{x} + 2 q_{y} q_{z} & 0 & 0 & 0\\\\- 2 q_{w} \\left(- p_{1} + s_{RR}1\\right) - 4 q_{x} \\left(- p_{2} + s_{RR}2\\right) + 2 q_{z} \\left(- p_{0} + s_{RR}0\\right) & 2 q_{w} \\left(- p_{0} + s_{RR}0\\right) - 4 q_{y} \\left(- p_{2} + s_{RR}2\\right) + 2 q_{z} \\left(- p_{1} + s_{RR}1\\right) & 2 q_{x} \\left(- p_{0} + s_{RR}0\\right) + 2 q_{y} \\left(- p_{1} + s_{RR}1\\right) & - 2 q_{x} \\left(- p_{1} + s_{RR}1\\right) + 2 q_{y} \\left(- p_{0} + s_{RR}0\\right) & - 2 q_{w} q_{y} - 2 q_{x} q_{z} & 2 q_{w} q_{x} - 2 q_{y} q_{z} & 2 q_{x}^{2} + 2 q_{y}^{2} - 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 2 q_{w} q_{y} + 2 q_{x} q_{z} & - 2 q_{w} q_{x} + 2 q_{y} q_{z} & - 2 q_{x}^{2} - 2 q_{y}^{2} + 1 & 0 & 0 & 0\\\\2 q_{y} \\left(- p_{1} + s_{RL}1\\right) + 2 q_{z} \\left(- p_{2} + s_{RL}2\\right) & - 2 q_{w} \\left(- p_{2} + s_{RL}2\\right) + 2 q_{x} \\left(- p_{1} + s_{RL}1\\right) - 4 q_{y} \\left(- p_{0} + s_{RL}0\\right) & 2 q_{w} \\left(- p_{1} + s_{RL}1\\right) + 2 q_{x} \\left(- p_{2} + s_{RL}2\\right) - 4 q_{z} \\left(- p_{0} + s_{RL}0\\right) & - 2 q_{y} \\left(- p_{2} + s_{RL}2\\right) + 2 q_{z} \\left(- p_{1} + s_{RL}1\\right) & 2 q_{y}^{2} + 2 q_{z}^{2} - 1 & - 2 q_{w} q_{z} - 2 q_{x} q_{y} & 2 q_{w} q_{y} - 2 q_{x} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 2 q_{y}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{w} q_{y} + 2 q_{x} q_{z}\\\\2 q_{w} \\left(- p_{2} + s_{RL}2\\right) - 4 q_{x} \\left(- p_{1} + s_{RL}1\\right) + 2 q_{y} \\left(- p_{0} + s_{RL}0\\right) & 2 q_{x} \\left(- p_{0} + s_{RL}0\\right) + 2 q_{z} \\left(- p_{2} + s_{RL}2\\right) & - 2 q_{w} \\left(- p_{0} + s_{RL}0\\right) + 2 q_{y} \\left(- p_{2} + s_{RL}2\\right) - 4 q_{z} \\left(- p_{1} + s_{RL}1\\right) & 2 q_{x} \\left(- p_{2} + s_{RL}2\\right) - 2 q_{z} \\left(- p_{0} + s_{RL}0\\right) & 2 q_{w} q_{z} - 2 q_{x} q_{y} & 2 q_{x}^{2} + 2 q_{z}^{2} - 1 & - 2 q_{w} q_{x} - 2 q_{y} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{x}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{x} + 2 q_{y} q_{z}\\\\- 2 q_{w} \\left(- p_{1} + s_{RL}1\\right) - 4 q_{x} \\left(- p_{2} + s_{RL}2\\right) + 2 q_{z} \\left(- p_{0} + s_{RL}0\\right) & 2 q_{w} \\left(- p_{0} + s_{RL}0\\right) - 4 q_{y} \\left(- p_{2} + s_{RL}2\\right) + 2 q_{z} \\left(- p_{1} + s_{RL}1\\right) & 2 q_{x} \\left(- p_{0} + s_{RL}0\\right) + 2 q_{y} \\left(- p_{1} + s_{RL}1\\right) & - 2 q_{x} \\left(- p_{1} + s_{RL}1\\right) + 2 q_{y} \\left(- p_{0} + s_{RL}0\\right) & - 2 q_{w} q_{y} - 2 q_{x} q_{z} & 2 q_{w} q_{x} - 2 q_{y} q_{z} & 2 q_{x}^{2} + 2 q_{y}^{2} - 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 2 q_{w} q_{y} + 2 q_{x} q_{z} & - 2 q_{w} q_{x} + 2 q_{y} q_{z} & - 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\end{array}\\right]$" + ], + "text/plain": [ + "⎡ \n", + "⎢ 2⋅q_y⋅v₁ + 2⋅q_z⋅v₂ \n", + "⎢ \n", + "⎢ \n", + "⎢ 2⋅q_w⋅v₂ - 4⋅qₓ⋅v₁ + 2⋅q_y⋅v₀ \n", + "⎢ \n", + "⎢ \n", + "⎢ -2⋅q_w⋅v₁ - 4⋅qₓ⋅v₂ + 2⋅q_z⋅v₀ \n", + "⎢ \n", + "⎢ \n", + "⎢ 2⋅q_y⋅(-p₁ + s_{FR}1) + 2⋅q_z⋅(-p₂ + s_{FR}2) -2⋅q_w\n", + "⎢ \n", + "⎢ \n", + "⎢2⋅q_w⋅(-p₂ + s_{FR}2) - 4⋅qₓ⋅(-p₁ + s_{FR}1) + 2⋅q_y⋅(-p₀ + s_{FR}0) \n", + "⎢ \n", + "⎢ \n", + "⎢-2⋅q_w⋅(-p₁ + s_{FR}1) - 4⋅qₓ⋅(-p₂ + s_{FR}2) + 2⋅q_z⋅(-p₀ + s_{FR}0) 2⋅q_w⋅\n", + "⎢ \n", + "⎢ \n", + "⎢ 2⋅q_y⋅(-p₁ + s_{FL}1) + 2⋅q_z⋅(-p₂ + s_{FL}2) -2⋅q_w\n", + "⎢ \n", + "⎢ \n", + "⎢2⋅q_w⋅(-p₂ + s_{FL}2) - 4⋅qₓ⋅(-p₁ + s_{FL}1) + 2⋅q_y⋅(-p₀ + s_{FL}0) \n", + "⎢ \n", + "⎢ \n", + "⎢-2⋅q_w⋅(-p₁ + s_{FL}1) - 4⋅qₓ⋅(-p₂ + s_{FL}2) + 2⋅q_z⋅(-p₀ + s_{FL}0) 2⋅q_w⋅\n", + "⎢ \n", + "⎢ \n", + "⎢ 2⋅q_y⋅(-p₁ + s_{RR}1) + 2⋅q_z⋅(-p₂ + s_{RR}2) -2⋅q_w\n", + "⎢ \n", + "⎢ \n", + "⎢2⋅q_w⋅(-p₂ + s_{RR}2) - 4⋅qₓ⋅(-p₁ + s_{RR}1) + 2⋅q_y⋅(-p₀ + s_{RR}0) \n", + "⎢ \n", + "⎢ \n", + "⎢-2⋅q_w⋅(-p₁ + s_{RR}1) - 4⋅qₓ⋅(-p₂ + s_{RR}2) + 2⋅q_z⋅(-p₀ + s_{RR}0) 2⋅q_w⋅\n", + "⎢ \n", + "⎢ \n", + "⎢ 2⋅q_y⋅(-p₁ + s_{RL}1) + 2⋅q_z⋅(-p₂ + s_{RL}2) -2⋅q_w\n", + "⎢ \n", + "⎢ \n", + "⎢2⋅q_w⋅(-p₂ + s_{RL}2) - 4⋅qₓ⋅(-p₁ + s_{RL}1) + 2⋅q_y⋅(-p₀ + s_{RL}0) \n", + "⎢ \n", + "⎢ \n", + "⎣-2⋅q_w⋅(-p₁ + s_{RL}1) - 4⋅qₓ⋅(-p₂ + s_{RL}2) + 2⋅q_z⋅(-p₀ + s_{RL}0) 2⋅q_w⋅\n", + "\n", + " \n", + " -2⋅q_w⋅v₂ + 2⋅qₓ⋅v₁ - 4⋅q_y⋅v₀ \n", + " \n", + " \n", + " 2⋅qₓ⋅v₀ + 2⋅q_z⋅v₂ \n", + " \n", + " \n", + " 2⋅q_w⋅v₀ - 4⋅q_y⋅v₂ + 2⋅q_z⋅v₁ \n", + " \n", + " \n", + "⋅(-p₂ + s_{FR}2) + 2⋅qₓ⋅(-p₁ + s_{FR}1) - 4⋅q_y⋅(-p₀ + s_{FR}0) 2⋅q_w⋅(-p₁ +\n", + " \n", + " \n", + " 2⋅qₓ⋅(-p₀ + s_{FR}0) + 2⋅q_z⋅(-p₂ + s_{FR}2) -2⋅q_w⋅(-p₀ +\n", + " \n", + " \n", + "(-p₀ + s_{FR}0) - 4⋅q_y⋅(-p₂ + s_{FR}2) + 2⋅q_z⋅(-p₁ + s_{FR}1) \n", + " \n", + " \n", + "⋅(-p₂ + s_{FL}2) + 2⋅qₓ⋅(-p₁ + s_{FL}1) - 4⋅q_y⋅(-p₀ + s_{FL}0) 2⋅q_w⋅(-p₁ +\n", + " \n", + " \n", + " 2⋅qₓ⋅(-p₀ + s_{FL}0) + 2⋅q_z⋅(-p₂ + s_{FL}2) -2⋅q_w⋅(-p₀ +\n", + " \n", + " \n", + "(-p₀ + s_{FL}0) - 4⋅q_y⋅(-p₂ + s_{FL}2) + 2⋅q_z⋅(-p₁ + s_{FL}1) \n", + " \n", + " \n", + "⋅(-p₂ + s_{RR}2) + 2⋅qₓ⋅(-p₁ + s_{RR}1) - 4⋅q_y⋅(-p₀ + s_{RR}0) 2⋅q_w⋅(-p₁ +\n", + " \n", + " \n", + " 2⋅qₓ⋅(-p₀ + s_{RR}0) + 2⋅q_z⋅(-p₂ + s_{RR}2) -2⋅q_w⋅(-p₀ +\n", + " \n", + " \n", + "(-p₀ + s_{RR}0) - 4⋅q_y⋅(-p₂ + s_{RR}2) + 2⋅q_z⋅(-p₁ + s_{RR}1) \n", + " \n", + " \n", + "⋅(-p₂ + s_{RL}2) + 2⋅qₓ⋅(-p₁ + s_{RL}1) - 4⋅q_y⋅(-p₀ + s_{RL}0) 2⋅q_w⋅(-p₁ +\n", + " \n", + " \n", + " 2⋅qₓ⋅(-p₀ + s_{RL}0) + 2⋅q_z⋅(-p₂ + s_{RL}2) -2⋅q_w⋅(-p₀ +\n", + " \n", + " \n", + "(-p₀ + s_{RL}0) - 4⋅q_y⋅(-p₂ + s_{RL}2) + 2⋅q_z⋅(-p₁ + s_{RL}1) \n", + "\n", + " \n", + " 2⋅q_w⋅v₁ + 2⋅qₓ⋅v₂ - 4⋅q_z⋅v₀ -2⋅q_y\n", + " \n", + " \n", + " -2⋅q_w⋅v₀ + 2⋅q_y⋅v₂ - 4⋅q_z⋅v₁ 2⋅qₓ⋅\n", + " \n", + " \n", + " 2⋅qₓ⋅v₀ + 2⋅q_y⋅v₁ -2⋅qₓ⋅\n", + " \n", + " \n", + " s_{FR}1) + 2⋅qₓ⋅(-p₂ + s_{FR}2) - 4⋅q_z⋅(-p₀ + s_{FR}0) -2⋅q_y⋅(-p₂ + s_{FR\n", + " \n", + " \n", + " s_{FR}0) + 2⋅q_y⋅(-p₂ + s_{FR}2) - 4⋅q_z⋅(-p₁ + s_{FR}1) 2⋅qₓ⋅(-p₂ + s_{FR}\n", + " \n", + " \n", + "2⋅qₓ⋅(-p₀ + s_{FR}0) + 2⋅q_y⋅(-p₁ + s_{FR}1) -2⋅qₓ⋅(-p₁ + s_{FR}\n", + " \n", + " \n", + " s_{FL}1) + 2⋅qₓ⋅(-p₂ + s_{FL}2) - 4⋅q_z⋅(-p₀ + s_{FL}0) -2⋅q_y⋅(-p₂ + s_{FL\n", + " \n", + " \n", + " s_{FL}0) + 2⋅q_y⋅(-p₂ + s_{FL}2) - 4⋅q_z⋅(-p₁ + s_{FL}1) 2⋅qₓ⋅(-p₂ + s_{FL}\n", + " \n", + " \n", + "2⋅qₓ⋅(-p₀ + s_{FL}0) + 2⋅q_y⋅(-p₁ + s_{FL}1) -2⋅qₓ⋅(-p₁ + s_{FL}\n", + " \n", + " \n", + " s_{RR}1) + 2⋅qₓ⋅(-p₂ + s_{RR}2) - 4⋅q_z⋅(-p₀ + s_{RR}0) -2⋅q_y⋅(-p₂ + s_{RR\n", + " \n", + " \n", + " s_{RR}0) + 2⋅q_y⋅(-p₂ + s_{RR}2) - 4⋅q_z⋅(-p₁ + s_{RR}1) 2⋅qₓ⋅(-p₂ + s_{RR}\n", + " \n", + " \n", + "2⋅qₓ⋅(-p₀ + s_{RR}0) + 2⋅q_y⋅(-p₁ + s_{RR}1) -2⋅qₓ⋅(-p₁ + s_{RR}\n", + " \n", + " \n", + " s_{RL}1) + 2⋅qₓ⋅(-p₂ + s_{RL}2) - 4⋅q_z⋅(-p₀ + s_{RL}0) -2⋅q_y⋅(-p₂ + s_{RL\n", + " \n", + " \n", + " s_{RL}0) + 2⋅q_y⋅(-p₂ + s_{RL}2) - 4⋅q_z⋅(-p₁ + s_{RL}1) 2⋅qₓ⋅(-p₂ + s_{RL}\n", + " \n", + " \n", + "2⋅qₓ⋅(-p₀ + s_{RL}0) + 2⋅q_y⋅(-p₁ + s_{RL}1) -2⋅qₓ⋅(-p₁ + s_{RL}\n", + "\n", + " \n", + "⋅v₂ + 2⋅q_z⋅v₁ 0 0 \n", + " \n", + " \n", + "v₂ - 2⋅q_z⋅v₀ 0 0 \n", + " \n", + " \n", + "v₁ + 2⋅q_y⋅v₀ 0 0 \n", + " \n", + " 2 2 \n", + "}2) + 2⋅q_z⋅(-p₁ + s_{FR}1) 2⋅q_y + 2⋅q_z - 1 -2⋅q_w⋅q_z - 2⋅qₓ⋅q_y 2⋅q\n", + " \n", + " 2 2 \n", + "2) - 2⋅q_z⋅(-p₀ + s_{FR}0) 2⋅q_w⋅q_z - 2⋅qₓ⋅q_y 2⋅qₓ + 2⋅q_z - 1 -2⋅\n", + " \n", + " \n", + "1) + 2⋅q_y⋅(-p₀ + s_{FR}0) -2⋅q_w⋅q_y - 2⋅qₓ⋅q_z 2⋅q_w⋅qₓ - 2⋅q_y⋅q_z 2⋅\n", + " \n", + " 2 2 \n", + "}2) + 2⋅q_z⋅(-p₁ + s_{FL}1) 2⋅q_y + 2⋅q_z - 1 -2⋅q_w⋅q_z - 2⋅qₓ⋅q_y 2⋅q\n", + " \n", + " 2 2 \n", + "2) - 2⋅q_z⋅(-p₀ + s_{FL}0) 2⋅q_w⋅q_z - 2⋅qₓ⋅q_y 2⋅qₓ + 2⋅q_z - 1 -2⋅\n", + " \n", + " \n", + "1) + 2⋅q_y⋅(-p₀ + s_{FL}0) -2⋅q_w⋅q_y - 2⋅qₓ⋅q_z 2⋅q_w⋅qₓ - 2⋅q_y⋅q_z 2⋅\n", + " \n", + " 2 2 \n", + "}2) + 2⋅q_z⋅(-p₁ + s_{RR}1) 2⋅q_y + 2⋅q_z - 1 -2⋅q_w⋅q_z - 2⋅qₓ⋅q_y 2⋅q\n", + " \n", + " 2 2 \n", + "2) - 2⋅q_z⋅(-p₀ + s_{RR}0) 2⋅q_w⋅q_z - 2⋅qₓ⋅q_y 2⋅qₓ + 2⋅q_z - 1 -2⋅\n", + " \n", + " \n", + "1) + 2⋅q_y⋅(-p₀ + s_{RR}0) -2⋅q_w⋅q_y - 2⋅qₓ⋅q_z 2⋅q_w⋅qₓ - 2⋅q_y⋅q_z 2⋅\n", + " \n", + " 2 2 \n", + "}2) + 2⋅q_z⋅(-p₁ + s_{RL}1) 2⋅q_y + 2⋅q_z - 1 -2⋅q_w⋅q_z - 2⋅qₓ⋅q_y 2⋅q\n", + " \n", + " 2 2 \n", + "2) - 2⋅q_z⋅(-p₀ + s_{RL}0) 2⋅q_w⋅q_z - 2⋅qₓ⋅q_y 2⋅qₓ + 2⋅q_z - 1 -2⋅\n", + " \n", + " \n", + "1) + 2⋅q_y⋅(-p₀ + s_{RL}0) -2⋅q_w⋅q_y - 2⋅qₓ⋅q_z 2⋅q_w⋅qₓ - 2⋅q_y⋅q_z 2⋅\n", + "\n", + " 2 2 \n", + " 0 - 2⋅q_y - 2⋅q_z + 1 2⋅q_w⋅q_z + 2⋅qₓ⋅q_y -2⋅q_w⋅q_y +\n", + " \n", + " 2 2 \n", + " 0 -2⋅q_w⋅q_z + 2⋅qₓ⋅q_y - 2⋅qₓ - 2⋅q_z + 1 2⋅q_w⋅qₓ + 2\n", + " \n", + " 2 \n", + " 0 2⋅q_w⋅q_y + 2⋅qₓ⋅q_z -2⋅q_w⋅qₓ + 2⋅q_y⋅q_z - 2⋅qₓ - 2⋅\n", + " \n", + " \n", + "_w⋅q_y - 2⋅qₓ⋅q_z 0 0 0 \n", + " \n", + " \n", + "q_w⋅qₓ - 2⋅q_y⋅q_z 0 0 0 \n", + " \n", + " 2 2 \n", + "qₓ + 2⋅q_y - 1 0 0 0 \n", + " \n", + " \n", + "_w⋅q_y - 2⋅qₓ⋅q_z 0 0 0 \n", + " \n", + " \n", + "q_w⋅qₓ - 2⋅q_y⋅q_z 0 0 0 \n", + " \n", + " 2 2 \n", + "qₓ + 2⋅q_y - 1 0 0 0 \n", + " \n", + " \n", + "_w⋅q_y - 2⋅qₓ⋅q_z 0 0 0 \n", + " \n", + " \n", + "q_w⋅qₓ - 2⋅q_y⋅q_z 0 0 0 \n", + " \n", + " 2 2 \n", + "qₓ + 2⋅q_y - 1 0 0 0 \n", + " \n", + " \n", + "_w⋅q_y - 2⋅qₓ⋅q_z 0 0 0 \n", + " \n", + " \n", + "q_w⋅qₓ - 2⋅q_y⋅q_z 0 0 0 \n", + " \n", + " 2 2 \n", + "qₓ + 2⋅q_y - 1 0 0 0 \n", + "\n", + " \n", + " 2⋅qₓ⋅q_z 0 0 0 0 0 0 0 0 \n", + " \n", + " \n", + "⋅q_y⋅q_z 0 0 0 0 0 0 0 0 \n", + " \n", + " 2 \n", + "q_y + 1 0 0 0 0 0 0 0 0 \n", + " \n", + " 2 2 \n", + " 0 0 0 0 0 0 - 2⋅q_y - 2⋅q_z + 1 2⋅q_w⋅q_z + 2⋅qₓ⋅q_y -2⋅\n", + " \n", + " 2 2 \n", + " 0 0 0 0 0 0 -2⋅q_w⋅q_z + 2⋅qₓ⋅q_y - 2⋅qₓ - 2⋅q_z + 1 2⋅q\n", + " \n", + " \n", + " 0 0 0 0 0 0 2⋅q_w⋅q_y + 2⋅qₓ⋅q_z -2⋅q_w⋅qₓ + 2⋅q_y⋅q_z - 2\n", + " \n", + " \n", + " 0 0 0 0 0 0 0 0 \n", + " \n", + " \n", + " 0 0 0 0 0 0 0 0 \n", + " \n", + " \n", + " 0 0 0 0 0 0 0 0 \n", + " \n", + " \n", + " 0 0 0 0 0 0 0 0 \n", + " \n", + " \n", + " 0 0 0 0 0 0 0 0 \n", + " \n", + " \n", + " 0 0 0 0 0 0 0 0 \n", + " \n", + " \n", + " 0 0 0 0 0 0 0 0 \n", + " \n", + " \n", + " 0 0 0 0 0 0 0 0 \n", + " \n", + " \n", + " 0 0 0 0 0 0 0 0 \n", + "\n", + " \n", + " 0 0 0 0 \n", + " \n", + " \n", + " 0 0 0 0 \n", + " \n", + " \n", + " 0 0 0 0 \n", + " \n", + " \n", + "q_w⋅q_y + 2⋅qₓ⋅q_z 0 0 0 \n", + " \n", + " \n", + "_w⋅qₓ + 2⋅q_y⋅q_z 0 0 0 \n", + " \n", + " 2 2 \n", + "⋅qₓ - 2⋅q_y + 1 0 0 0 \n", + " \n", + " 2 2 \n", + " 0 - 2⋅q_y - 2⋅q_z + 1 2⋅q_w⋅q_z + 2⋅qₓ⋅q_y -2⋅q_w⋅q_y +\n", + " \n", + " 2 2 \n", + " 0 -2⋅q_w⋅q_z + 2⋅qₓ⋅q_y - 2⋅qₓ - 2⋅q_z + 1 2⋅q_w⋅qₓ + 2\n", + " \n", + " 2 \n", + " 0 2⋅q_w⋅q_y + 2⋅qₓ⋅q_z -2⋅q_w⋅qₓ + 2⋅q_y⋅q_z - 2⋅qₓ - 2⋅\n", + " \n", + " \n", + " 0 0 0 0 \n", + " \n", + " \n", + " 0 0 0 0 \n", + " \n", + " \n", + " 0 0 0 0 \n", + " \n", + " \n", + " 0 0 0 0 \n", + " \n", + " \n", + " 0 0 0 0 \n", + " \n", + " \n", + " 0 0 0 0 \n", + "\n", + " \n", + " 0 0 0 \n", + " \n", + " \n", + " 0 0 0 \n", + " \n", + " \n", + " 0 0 0 \n", + " \n", + " \n", + " 0 0 0 \n", + " \n", + " \n", + " 0 0 0 \n", + " \n", + " \n", + " 0 0 0 \n", + " \n", + " \n", + " 2⋅qₓ⋅q_z 0 0 0 \n", + " \n", + " \n", + "⋅q_y⋅q_z 0 0 0 \n", + " \n", + " 2 \n", + "q_y + 1 0 0 0 \n", + " \n", + " 2 2 \n", + " - 2⋅q_y - 2⋅q_z + 1 2⋅q_w⋅q_z + 2⋅qₓ⋅q_y -2⋅q_w⋅q_y + 2⋅qₓ⋅q_z\n", + " \n", + " 2 2 \n", + " -2⋅q_w⋅q_z + 2⋅qₓ⋅q_y - 2⋅qₓ - 2⋅q_z + 1 2⋅q_w⋅qₓ + 2⋅q_y⋅q_z \n", + " \n", + " 2 2 \n", + " 2⋅q_w⋅q_y + 2⋅qₓ⋅q_z -2⋅q_w⋅qₓ + 2⋅q_y⋅q_z - 2⋅qₓ - 2⋅q_y + 1 \n", + " \n", + " \n", + " 0 0 0 \n", + " \n", + " \n", + " 0 0 0 \n", + " \n", + " \n", + " 0 0 0 \n", + "\n", + " ⎤\n", + " 0 0 0 ⎥\n", + " ⎥\n", + " ⎥\n", + " 0 0 0 ⎥\n", + " ⎥\n", + " ⎥\n", + " 0 0 0 ⎥\n", + " ⎥\n", + " ⎥\n", + " 0 0 0 ⎥\n", + " ⎥\n", + " ⎥\n", + " 0 0 0 ⎥\n", + " ⎥\n", + " ⎥\n", + " 0 0 0 ⎥\n", + " ⎥\n", + " ⎥\n", + " 0 0 0 ⎥\n", + " ⎥\n", + " ⎥\n", + " 0 0 0 ⎥\n", + " ⎥\n", + " ⎥\n", + " 0 0 0 ⎥\n", + " ⎥\n", + " ⎥\n", + " 0 0 0 ⎥\n", + " ⎥\n", + " ⎥\n", + " 0 0 0 ⎥\n", + " ⎥\n", + " ⎥\n", + " 0 0 0 ⎥\n", + " ⎥\n", + " 2 2 ⎥\n", + " - 2⋅q_y - 2⋅q_z + 1 2⋅q_w⋅q_z + 2⋅qₓ⋅q_y -2⋅q_w⋅q_y + 2⋅qₓ⋅q_z⎥\n", + " ⎥\n", + " 2 2 ⎥\n", + " -2⋅q_w⋅q_z + 2⋅qₓ⋅q_y - 2⋅qₓ - 2⋅q_z + 1 2⋅q_w⋅qₓ + 2⋅q_y⋅q_z ⎥\n", + " ⎥\n", + " 2 2 ⎥\n", + " 2⋅q_w⋅q_y + 2⋅qₓ⋅q_z -2⋅q_w⋅qₓ + 2⋅q_y⋅q_z - 2⋅qₓ - 2⋅q_y + 1 ⎦" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "H = h.jacobian(state)\n", + "H" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\left[\\begin{array}{ccccccccccccccc}R_{v} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & R_{v} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & R_{v} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & R_{s_{FR}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & R_{s_{FR}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & R_{s_{FR}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & R_{s_{FL}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & R_{s_{FL}} & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & R_{s_{FL}} & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & R_{s_{RR}} & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & R_{s_{RR}} & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & R_{s_{RR}} & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & R_{s_{RL}} & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & R_{s_{RL}} & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & R_{s_{RL}}\\end{array}\\right]$" + ], + "text/plain": [ + "⎡Rᵥ 0 0 0 0 0 0 0 0\n", + "⎢ \n", + "⎢0 Rᵥ 0 0 0 0 0 0 0\n", + "⎢ \n", + "⎢0 0 Rᵥ 0 0 0 0 0 0\n", + "⎢ \n", + "⎢0 0 0 R_{s_{FR}} 0 0 0 0 0\n", + "⎢ \n", + "⎢0 0 0 0 R_{s_{FR}} 0 0 0 0\n", + "⎢ \n", + "⎢0 0 0 0 0 R_{s_{FR}} 0 0 0\n", + "⎢ \n", + "⎢0 0 0 0 0 0 R_{s_{FL}} 0 0\n", + "⎢ \n", + "⎢0 0 0 0 0 0 0 R_{s_{FL}} 0\n", + "⎢ \n", + "⎢0 0 0 0 0 0 0 0 R_{s_\n", + "⎢ \n", + "⎢0 0 0 0 0 0 0 0 0\n", + "⎢ \n", + "⎢0 0 0 0 0 0 0 0 0\n", + "⎢ \n", + "⎢0 0 0 0 0 0 0 0 0\n", + "⎢ \n", + "⎢0 0 0 0 0 0 0 0 0\n", + "⎢ \n", + "⎢0 0 0 0 0 0 0 0 0\n", + "⎢ \n", + "⎣0 0 0 0 0 0 0 0 0\n", + "\n", + " 0 0 0 0 0 0 ⎤\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + "{FL}} 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " R_{s_{RR}} 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 R_{s_{RR}} 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 R_{s_{RR}} 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 R_{s_{RL}} 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 R_{s_{RL}} 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 R_{s_{RL}}⎦" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "R_vel = sf.Symbol('R_v')\n", + "R_s1 = sf.Symbol('R_{s_{FR}}')\n", + "R_s2 = sf.Symbol('R_{s_{FL}}')\n", + "R_s3 = sf.Symbol('R_{s_{RR}}')\n", + "R_s4 = sf.Symbol('R_{s_{RL}}')\n", + "\n", + "R_cov = sf.Matrix.diag([R_vel,R_vel,R_vel,\n", + " R_s1, R_s1, R_s1,\n", + " R_s2, R_s2, R_s2,\n", + " R_s3, R_s3, R_s3,\n", + " R_s4, R_s4, R_s4])\n", + "R_cov" + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\left[\\begin{array}{cccccccccccccccccccccccccccc}Q_{q} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & Q_{q} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & Q_{q} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & Q_{q} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & Q_{p} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & Q_{p} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & Q_{p} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{v} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{v} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{v} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{bg} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{bg} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{bg} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{ba} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{ba} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{ba} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{FR}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{FR}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{FR}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{FL}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{FL}} & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{FL}} & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{RR}} & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{RR}} & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{RR}} & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{RL}} & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{RL}} & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{RL}}\\end{array}\\right]$" + ], + "text/plain": [ + "⎡Q_q 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 Q_q 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 Q_q 0 0 0 0 0 0 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 Q_q 0 0 0 0 0 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 Qₚ 0 0 0 0 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 Qₚ 0 0 0 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 Qₚ 0 0 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 Qᵥ 0 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 0 Qᵥ 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 0 0 Qᵥ 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 0 0 0 Q_bg 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 0 0 0 0 Q_bg 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 0 0 0 0 0 Q_bg 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 Q_ba 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 Q_ba 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 Q_b\n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n", + "⎢ \n", + "⎣ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n", + "\n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + "a 0 0 0 0 0 0 \n", + " \n", + " Q_{s_{FR}} 0 0 0 0 0 \n", + " \n", + " 0 Q_{s_{FR}} 0 0 0 0 \n", + " \n", + " 0 0 Q_{s_{FR}} 0 0 0 \n", + " \n", + " 0 0 0 Q_{s_{FL}} 0 0 \n", + " \n", + " 0 0 0 0 Q_{s_{FL}} 0 \n", + " \n", + " 0 0 0 0 0 Q_{s_{FL}} \n", + " \n", + " 0 0 0 0 0 0 Q_{\n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + " \n", + " 0 0 0 0 0 0 \n", + "\n", + " 0 0 0 0 0 0 ⎤\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 0 ⎥\n", + " ⎥\n", + "s_{RR}} 0 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 Q_{s_{RR}} 0 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 Q_{s_{RR}} 0 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 Q_{s_{RL}} 0 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 Q_{s_{RL}} 0 ⎥\n", + " ⎥\n", + " 0 0 0 0 0 Q_{s_{RL}}⎦" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "\n", + "Q_vel = sf.Symbol('Q_v')\n", + "Q_pos = sf.Symbol('Q_p')\n", + "Q_quat = sf.Symbol('Q_q')\n", + "Q_bg = sf.Symbol('Q_bg')\n", + "Q_ba = sf.Symbol('Q_ba')\n", + "Q_s1 = sf.Symbol('Q_{s_{FR}}')\n", + "Q_s2 = sf.Symbol('Q_{s_{FL}}')\n", + "Q_s3 = sf.Symbol('Q_{s_{RR}}')\n", + "Q_s4 = sf.Symbol('Q_{s_{RL}}')\n", + "\n", + "Q_cov = sf.Matrix.diag([Q_quat,Q_quat,Q_quat,Q_quat,\n", + " Q_pos, Q_pos, Q_pos,\n", + " Q_vel, Q_vel, Q_vel,\n", + " Q_bg, Q_bg, Q_bg,\n", + " Q_ba, Q_ba, Q_ba,\n", + " Q_s1, Q_s1, Q_s1,\n", + " Q_s2, Q_s2, Q_s2,\n", + " Q_s3, Q_s3, Q_s3,\n", + " Q_s4, Q_s4, Q_s4])\n", + "Q_cov\n" + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\left[\\begin{matrix}q_{x}\\\\q_{y}\\\\q_{z}\\\\q_{w}\\\\p_{0}\\\\p_{1}\\\\p_{2}\\\\v_{0}\\\\v_{1}\\\\v_{2}\\\\b_{g0}\\\\b_{g1}\\\\b_{g2}\\\\b_{a0}\\\\b_{a1}\\\\b_{a2}\\\\s_{FR}0\\\\s_{FR}1\\\\s_{FR}2\\\\s_{FL}0\\\\s_{FL}1\\\\s_{FL}2\\\\s_{RR}0\\\\s_{RR}1\\\\s_{RR}2\\\\s_{RL}0\\\\s_{RL}1\\\\s_{RL}2\\end{matrix}\\right]$" + ], + "text/plain": [ + "⎡ qₓ ⎤\n", + "⎢ ⎥\n", + "⎢ q_y ⎥\n", + "⎢ ⎥\n", + "⎢ q_z ⎥\n", + "⎢ ⎥\n", + "⎢ q_w ⎥\n", + "⎢ ⎥\n", + "⎢ p₀ ⎥\n", + "⎢ ⎥\n", + "⎢ p₁ ⎥\n", + "⎢ ⎥\n", + "⎢ p₂ ⎥\n", + "⎢ ⎥\n", + "⎢ v₀ ⎥\n", + "⎢ ⎥\n", + "⎢ v₁ ⎥\n", + "⎢ ⎥\n", + "⎢ v₂ ⎥\n", + "⎢ ⎥\n", + "⎢ b_g0 ⎥\n", + "⎢ ⎥\n", + "⎢ b_g1 ⎥\n", + "⎢ ⎥\n", + "⎢ b_g2 ⎥\n", + "⎢ ⎥\n", + "⎢ bₐ₀ ⎥\n", + "⎢ ⎥\n", + "⎢ bₐ₁ ⎥\n", + "⎢ ⎥\n", + "⎢ bₐ₂ ⎥\n", + "⎢ ⎥\n", + "⎢s_{FR}0⎥\n", + "⎢ ⎥\n", + "⎢s_{FR}1⎥\n", + "⎢ ⎥\n", + "⎢s_{FR}2⎥\n", + "⎢ ⎥\n", + "⎢s_{FL}0⎥\n", + "⎢ ⎥\n", + "⎢s_{FL}1⎥\n", + "⎢ ⎥\n", + "⎢s_{FL}2⎥\n", + "⎢ ⎥\n", + "⎢s_{RR}0⎥\n", + "⎢ ⎥\n", + "⎢s_{RR}1⎥\n", + "⎢ ⎥\n", + "⎢s_{RR}2⎥\n", + "⎢ ⎥\n", + "⎢s_{RL}0⎥\n", + "⎢ ⎥\n", + "⎢s_{RL}1⎥\n", + "⎢ ⎥\n", + "⎣s_{RL}2⎦" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "state" + ] + }, + { + "cell_type": "code", + "execution_count": 14, + "metadata": {}, + "outputs": [], + "source": [ + "P = sf.matrix_type_from_shape((28,28)).symbolic('P')" + ] + }, + { + "cell_type": "code", + "execution_count": 15, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\left[\\begin{matrix}q_{x}\\\\q_{y}\\\\q_{z}\\\\q_{w}\\\\p_{0}\\\\p_{1}\\\\p_{2}\\\\v_{0}\\\\v_{1}\\\\v_{2}\\\\b_{g0}\\\\b_{g1}\\\\b_{g2}\\\\b_{a0}\\\\b_{a1}\\\\b_{a2}\\\\s_{FR}0\\\\s_{FR}1\\\\s_{FR}2\\\\s_{FL}0\\\\s_{FL}1\\\\s_{FL}2\\\\s_{RR}0\\\\s_{RR}1\\\\s_{RR}2\\\\s_{RL}0\\\\s_{RL}1\\\\s_{RL}2\\end{matrix}\\right]$" + ], + "text/plain": [ + "⎡ qₓ ⎤\n", + "⎢ ⎥\n", + "⎢ q_y ⎥\n", + "⎢ ⎥\n", + "⎢ q_z ⎥\n", + "⎢ ⎥\n", + "⎢ q_w ⎥\n", + "⎢ ⎥\n", + "⎢ p₀ ⎥\n", + "⎢ ⎥\n", + "⎢ p₁ ⎥\n", + "⎢ ⎥\n", + "⎢ p₂ ⎥\n", + "⎢ ⎥\n", + "⎢ v₀ ⎥\n", + "⎢ ⎥\n", + "⎢ v₁ ⎥\n", + "⎢ ⎥\n", + "⎢ v₂ ⎥\n", + "⎢ ⎥\n", + "⎢ b_g0 ⎥\n", + "⎢ ⎥\n", + "⎢ b_g1 ⎥\n", + "⎢ ⎥\n", + "⎢ b_g2 ⎥\n", + "⎢ ⎥\n", + "⎢ bₐ₀ ⎥\n", + "⎢ ⎥\n", + "⎢ bₐ₁ ⎥\n", + "⎢ ⎥\n", + "⎢ bₐ₂ ⎥\n", + "⎢ ⎥\n", + "⎢s_{FR}0⎥\n", + "⎢ ⎥\n", + "⎢s_{FR}1⎥\n", + "⎢ ⎥\n", + "⎢s_{FR}2⎥\n", + "⎢ ⎥\n", + "⎢s_{FL}0⎥\n", + "⎢ ⎥\n", + "⎢s_{FL}1⎥\n", + "⎢ ⎥\n", + "⎢s_{FL}2⎥\n", + "⎢ ⎥\n", + "⎢s_{RR}0⎥\n", + "⎢ ⎥\n", + "⎢s_{RR}1⎥\n", + "⎢ ⎥\n", + "⎢s_{RR}2⎥\n", + "⎢ ⎥\n", + "⎢s_{RL}0⎥\n", + "⎢ ⎥\n", + "⎢s_{RL}1⎥\n", + "⎢ ⎥\n", + "⎣s_{RL}2⎦" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "state = sf.Matrix.block_matrix([[sf.Matrix([q.x, q.y, q.z, q.w])],\\\n", + " [p],\\\n", + " [v],\\\n", + " [b_g],\\\n", + " [b_a],\\\n", + " [s1],\\\n", + " [s2],\\\n", + " [s3],\\\n", + " [s4]])\n", + "state" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Generate Codes" + ] + }, + { + "cell_type": "code", + "execution_count": 16, + "metadata": {}, + "outputs": [], + "source": [ + "def compute_mean( state: sf.matrix_type_from_shape((28,1)), u: sf.matrix_type_from_shape((6,1)), dT: sf.Scalar = 0, epsilon: sf.Scalar = 0) -> sf.matrix_type_from_shape((28,1)):\n", + " omega = u[0:3]\n", + " accel = u[3:6]\n", + " q = sf.Quaternion(xyz=state[0:3], w=state[3])\n", + " p = state[4:7]\n", + " v = state[7:10]\n", + " b_g = state[10:13]\n", + " b_a = state[13:16]\n", + " s1 = state[16:19]\n", + " s2 = state[19:22]\n", + " s3 = state[22:25]\n", + " s4 = state[25:28]\n", + " f1 = q*sf.Quaternion(xyz=(omega-b_g)*dT, w = 1.)\n", + " # f1 = q*sf.Rot3.from_tangent((gyro-b_g)*dT, epsilon=epsilon).q\n", + " f1 = sf.Matrix([f1.x, f1.y, f1.z, f1.w])\n", + " f2 = p+dT*v\n", + " f3 = v+sf.Rot3(q).to_rotation_matrix()*(accel-b_a)-gravity\n", + " f4 = b_g\n", + " f5 = b_a\n", + " f6 = s1\n", + " f7 = s2\n", + " f8 = s3\n", + " f9 = s4\n", + " f = sf.Matrix.block_matrix([[f1],\\\n", + " [f2],\\\n", + " [f3],\\\n", + " [f4],\\\n", + " [f5],\\\n", + " [f6],\\\n", + " [f7],\\\n", + " [f8],\\\n", + " [f9]])\n", + " return f" + ] + }, + { + "cell_type": "code", + "execution_count": 17, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\left[\\begin{matrix}\\Delta t q_{w} \\left(\\omega0 - b_{g0}\\right) + \\Delta t q_{y} \\left(\\omega2 - b_{g2}\\right) - \\Delta t q_{z} \\left(\\omega1 - b_{g1}\\right) + 1.0 q_{x}\\\\\\Delta t q_{w} \\left(\\omega1 - b_{g1}\\right) - \\Delta t q_{x} \\left(\\omega2 - b_{g2}\\right) + \\Delta t q_{z} \\left(\\omega0 - b_{g0}\\right) + 1.0 q_{y}\\\\\\Delta t q_{w} \\left(\\omega2 - b_{g2}\\right) + \\Delta t q_{x} \\left(\\omega1 - b_{g1}\\right) - \\Delta t q_{y} \\left(\\omega0 - b_{g0}\\right) + 1.0 q_{z}\\\\- \\Delta t q_{x} \\left(\\omega0 - b_{g0}\\right) - \\Delta t q_{y} \\left(\\omega1 - b_{g1}\\right) - \\Delta t q_{z} \\left(\\omega2 - b_{g2}\\right) + 1.0 q_{w}\\\\\\Delta t v_{0} + p_{0}\\\\\\Delta t v_{1} + p_{1}\\\\\\Delta t v_{2} + p_{2}\\\\v_{0} + \\left(a_{0} - b_{a0}\\right) \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(a_{1} - b_{a1}\\right) \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(a_{2} - b_{a2}\\right) \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\v_{1} + \\left(a_{0} - b_{a0}\\right) \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(a_{1} - b_{a1}\\right) \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(a_{2} - b_{a2}\\right) \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\v_{2} + \\left(a_{0} - b_{a0}\\right) \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + \\left(a_{1} - b_{a1}\\right) \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + \\left(a_{2} - b_{a2}\\right) \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right) + 9.8\\\\b_{g0}\\\\b_{g1}\\\\b_{g2}\\\\b_{a0}\\\\b_{a1}\\\\b_{a2}\\\\s_{FR}0\\\\s_{FR}1\\\\s_{FR}2\\\\s_{FL}0\\\\s_{FL}1\\\\s_{FL}2\\\\s_{RR}0\\\\s_{RR}1\\\\s_{RR}2\\\\s_{RL}0\\\\s_{RL}1\\\\s_{RL}2\\end{matrix}\\right]$" + ], + "text/plain": [ + "⎡ \\Delta t⋅q_w⋅(\\omega0 - b_g0) + \\Delta t⋅q_y⋅(\\omega2 - b_g2) - \\Delta\n", + "⎢ \n", + "⎢ \\Delta t⋅q_w⋅(\\omega1 - b_g1) - \\Delta t⋅qₓ⋅(\\omega2 - b_g2) + \\Delta \n", + "⎢ \n", + "⎢ \\Delta t⋅q_w⋅(\\omega2 - b_g2) + \\Delta t⋅qₓ⋅(\\omega1 - b_g1) - \\Delta \n", + "⎢ \n", + "⎢ -\\Delta t⋅qₓ⋅(\\omega0 - b_g0) - \\Delta t⋅q_y⋅(\\omega1 - b_g1) - \\Delta\n", + "⎢ \n", + "⎢ \\Delta t⋅v₀ + p₀ \n", + "⎢ \n", + "⎢ \\Delta t⋅v₁ + p₁ \n", + "⎢ \n", + "⎢ \\Delta t⋅v₂ + p₂ \n", + "⎢ \n", + "⎢ ⎛ 2 2 ⎞ \n", + "⎢ v₀ + (a₀ - bₐ₀)⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + (a₁ - bₐ₁)⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y\n", + "⎢ \n", + "⎢ ⎛ 2 2 ⎞\n", + "⎢ v₁ + (a₀ - bₐ₀)⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + (a₁ - bₐ₁)⋅⎝- 2⋅qₓ - 2⋅q_z + 1⎠\n", + "⎢ \n", + "⎢ \n", + "⎢v₂ + (a₀ - bₐ₀)⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + (a₁ - bₐ₁)⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) +\n", + "⎢ \n", + "⎢ b_g0 \n", + "⎢ \n", + "⎢ b_g1 \n", + "⎢ \n", + "⎢ b_g2 \n", + "⎢ \n", + "⎢ bₐ₀ \n", + "⎢ \n", + "⎢ bₐ₁ \n", + "⎢ \n", + "⎢ bₐ₂ \n", + "⎢ \n", + "⎢ s_{FR}0 \n", + "⎢ \n", + "⎢ s_{FR}1 \n", + "⎢ \n", + "⎢ s_{FR}2 \n", + "⎢ \n", + "⎢ s_{FL}0 \n", + "⎢ \n", + "⎢ s_{FL}1 \n", + "⎢ \n", + "⎢ s_{FL}2 \n", + "⎢ \n", + "⎢ s_{RR}0 \n", + "⎢ \n", + "⎢ s_{RR}1 \n", + "⎢ \n", + "⎢ s_{RR}2 \n", + "⎢ \n", + "⎢ s_{RL}0 \n", + "⎢ \n", + "⎢ s_{RL}1 \n", + "⎢ \n", + "⎣ s_{RL}2 \n", + "\n", + " t⋅q_z⋅(\\omega1 - b_g1) + 1.0⋅qₓ ⎤\n", + " ⎥\n", + "t⋅q_z⋅(\\omega0 - b_g0) + 1.0⋅q_y ⎥\n", + " ⎥\n", + "t⋅q_y⋅(\\omega0 - b_g0) + 1.0⋅q_z ⎥\n", + " ⎥\n", + " t⋅q_z⋅(\\omega2 - b_g2) + 1.0⋅q_w ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + ") + (a₂ - bₐ₂)⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) ⎥\n", + " ⎥\n", + " ⎥\n", + " + (a₂ - bₐ₂)⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n", + " ⎥\n", + " ⎛ 2 2 ⎞ ⎥\n", + " (a₂ - bₐ₂)⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ + 9.8⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎥\n", + " ⎦" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "f_pred = compute_mean(state, u, dT)\n", + "f_pred" + ] + }, + { + "cell_type": "code", + "execution_count": 18, + "metadata": {}, + "outputs": [], + "source": [ + "def compute_measurement( state: sf.matrix_type_from_shape((28,1)), epsilon: sf.Scalar = 0)\\\n", + " -> sf.matrix_type_from_shape((15,1)):\n", + " q = sf.Quaternion(xyz=state[0:3], w=state[3])\n", + " p = state[4:7]\n", + " v = state[7:10]\n", + " b_g = state[10:13]\n", + " b_a = state[13:16]\n", + " s1 = state[16:19]\n", + " s2 = state[19:22]\n", + " s3 = state[22:25]\n", + " s4 = state[25:28]\n", + " R = sf.Rot3(q).to_rotation_matrix()\n", + " z1 = R.T*v\n", + " z2 = R.T*(s1-p)\n", + " z3 = R.T*(s2-p)\n", + " z4 = R.T*(s3-p)\n", + " z5 = R.T*(s4-p)\n", + " h = sf.Matrix.block_matrix([[z1],\n", + " [z2],\n", + " [z3],\n", + " [z4],\n", + " [z5]])\n", + " return h" + ] + }, + { + "cell_type": "code", + "execution_count": 19, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\left[\\begin{matrix}v_{0} \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + v_{1} \\cdot \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + v_{2} \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\v_{0} \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + v_{1} \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + v_{2} \\cdot \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\v_{0} \\cdot \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + v_{1} \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + v_{2} \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\\\\\left(- p_{0} + s_{FR}0\\right) \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{1} + s_{FR}1\\right) \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{2} + s_{FR}2\\right) \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\\\left(- p_{0} + s_{FR}0\\right) \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{1} + s_{FR}1\\right) \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{2} + s_{FR}2\\right) \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\\\left(- p_{0} + s_{FR}0\\right) \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + \\left(- p_{1} + s_{FR}1\\right) \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + \\left(- p_{2} + s_{FR}2\\right) \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\\\\\left(- p_{0} + s_{FL}0\\right) \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{1} + s_{FL}1\\right) \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{2} + s_{FL}2\\right) \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\\\left(- p_{0} + s_{FL}0\\right) \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{1} + s_{FL}1\\right) \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{2} + s_{FL}2\\right) \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\\\left(- p_{0} + s_{FL}0\\right) \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + \\left(- p_{1} + s_{FL}1\\right) \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + \\left(- p_{2} + s_{FL}2\\right) \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\\\\\left(- p_{0} + s_{RR}0\\right) \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{1} + s_{RR}1\\right) \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{2} + s_{RR}2\\right) \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\\\left(- p_{0} + s_{RR}0\\right) \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{1} + s_{RR}1\\right) \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{2} + s_{RR}2\\right) \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\\\left(- p_{0} + s_{RR}0\\right) \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + \\left(- p_{1} + s_{RR}1\\right) \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + \\left(- p_{2} + s_{RR}2\\right) \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\\\\\left(- p_{0} + s_{RL}0\\right) \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{1} + s_{RL}1\\right) \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{2} + s_{RL}2\\right) \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\\\left(- p_{0} + s_{RL}0\\right) \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{1} + s_{RL}1\\right) \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{2} + s_{RL}2\\right) \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\\\left(- p_{0} + s_{RL}0\\right) \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + \\left(- p_{1} + s_{RL}1\\right) \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + \\left(- p_{2} + s_{RL}2\\right) \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\end{matrix}\\right]$" + ], + "text/plain": [ + "⎡ ⎛ 2 2 ⎞ \n", + "⎢ v₀⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + v₁⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + v\n", + "⎢ \n", + "⎢ ⎛ 2 2 ⎞ \n", + "⎢ v₀⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + v₁⋅⎝- 2⋅qₓ - 2⋅q_z + 1⎠ + \n", + "⎢ \n", + "⎢ \n", + "⎢ v₀⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + v₁⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) + \n", + "⎢ \n", + "⎢ ⎛ 2 2 ⎞ \n", + "⎢(-p₀ + s_{FR}0)⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + (-p₁ + s_{FR}1)⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q\n", + "⎢ \n", + "⎢ ⎛ 2 2 \n", + "⎢(-p₀ + s_{FR}0)⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + (-p₁ + s_{FR}1)⋅⎝- 2⋅qₓ - 2⋅q_z +\n", + "⎢ \n", + "⎢ \n", + "⎢(-p₀ + s_{FR}0)⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + (-p₁ + s_{FR}1)⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q\n", + "⎢ \n", + "⎢ ⎛ 2 2 ⎞ \n", + "⎢(-p₀ + s_{FL}0)⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + (-p₁ + s_{FL}1)⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q\n", + "⎢ \n", + "⎢ ⎛ 2 2 \n", + "⎢(-p₀ + s_{FL}0)⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + (-p₁ + s_{FL}1)⋅⎝- 2⋅qₓ - 2⋅q_z +\n", + "⎢ \n", + "⎢ \n", + "⎢(-p₀ + s_{FL}0)⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + (-p₁ + s_{FL}1)⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q\n", + "⎢ \n", + "⎢ ⎛ 2 2 ⎞ \n", + "⎢(-p₀ + s_{RR}0)⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + (-p₁ + s_{RR}1)⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q\n", + "⎢ \n", + "⎢ ⎛ 2 2 \n", + "⎢(-p₀ + s_{RR}0)⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + (-p₁ + s_{RR}1)⋅⎝- 2⋅qₓ - 2⋅q_z +\n", + "⎢ \n", + "⎢ \n", + "⎢(-p₀ + s_{RR}0)⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + (-p₁ + s_{RR}1)⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q\n", + "⎢ \n", + "⎢ ⎛ 2 2 ⎞ \n", + "⎢(-p₀ + s_{RL}0)⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + (-p₁ + s_{RL}1)⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q\n", + "⎢ \n", + "⎢ ⎛ 2 2 \n", + "⎢(-p₀ + s_{RL}0)⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + (-p₁ + s_{RL}1)⋅⎝- 2⋅qₓ - 2⋅q_z +\n", + "⎢ \n", + "⎢ \n", + "⎣(-p₀ + s_{RL}0)⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + (-p₁ + s_{RL}1)⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q\n", + "\n", + " ⎤\n", + "₂⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) ⎥\n", + " ⎥\n", + " ⎥\n", + "v₂⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n", + " ⎥\n", + " ⎛ 2 2 ⎞ ⎥\n", + "v₂⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎥\n", + " ⎥\n", + " ⎥\n", + "_y) + (-p₂ + s_{FR}2)⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z)⎥\n", + " ⎥\n", + " ⎞ ⎥\n", + " 1⎠ + (-p₂ + s_{FR}2)⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n", + " ⎥\n", + " ⎛ 2 2 ⎞ ⎥\n", + "_z) + (-p₂ + s_{FR}2)⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎥\n", + " ⎥\n", + " ⎥\n", + "_y) + (-p₂ + s_{FL}2)⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z)⎥\n", + " ⎥\n", + " ⎞ ⎥\n", + " 1⎠ + (-p₂ + s_{FL}2)⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n", + " ⎥\n", + " ⎛ 2 2 ⎞ ⎥\n", + "_z) + (-p₂ + s_{FL}2)⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎥\n", + " ⎥\n", + " ⎥\n", + "_y) + (-p₂ + s_{RR}2)⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z)⎥\n", + " ⎥\n", + " ⎞ ⎥\n", + " 1⎠ + (-p₂ + s_{RR}2)⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n", + " ⎥\n", + " ⎛ 2 2 ⎞ ⎥\n", + "_z) + (-p₂ + s_{RR}2)⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎥\n", + " ⎥\n", + " ⎥\n", + "_y) + (-p₂ + s_{RL}2)⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z)⎥\n", + " ⎥\n", + " ⎞ ⎥\n", + " 1⎠ + (-p₂ + s_{RL}2)⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n", + " ⎥\n", + " ⎛ 2 2 ⎞ ⎥\n", + "_z) + (-p₂ + s_{RL}2)⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎦" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "compute_measurement(state)" + ] + }, + { + "cell_type": "code", + "execution_count": 20, + "metadata": {}, + "outputs": [], + "source": [ + "from symforce import codegen\n", + "from symforce.codegen import codegen_util\n", + "from symforce.notebook_util import display\n", + "from symforce.notebook_util import display_code_file\n", + "from symforce.values import Values" + ] + }, + { + "cell_type": "code", + "execution_count": 21, + "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "codegen.__init__():141 WARNING -- \n", + " Generating code with epsilon set to 0 - This is dangerous! You may get NaNs, Infs,\n", + " or numerically unstable results from calling generated functions near singularities.\n", + "\n", + " In order to safely generate code, you should set epsilon to either a symbol\n", + " (recommended) or a small numerical value like `sf.numeric_epsilon`. You should do\n", + " this before importing any other code from symforce, e.g. with\n", + "\n", + " import symforce\n", + " symforce.set_epsilon_to_symbol()\n", + "\n", + " or\n", + "\n", + " import symforce\n", + " symforce.set_epsilon_to_number()\n", + "\n", + " For more information on use of epsilon to prevent singularities, take a look at the\n", + " Epsilon Tutorial: https://symforce.org/tutorials/epsilon_tutorial.html\n", + "\n", + "codegen.__init__():141 WARNING -- \n", + " Generating code with epsilon set to 0 - This is dangerous! You may get NaNs, Infs,\n", + " or numerically unstable results from calling generated functions near singularities.\n", + "\n", + " In order to safely generate code, you should set epsilon to either a symbol\n", + " (recommended) or a small numerical value like `sf.numeric_epsilon`. You should do\n", + " this before importing any other code from symforce, e.g. with\n", + "\n", + " import symforce\n", + " symforce.set_epsilon_to_symbol()\n", + "\n", + " or\n", + "\n", + " import symforce\n", + " symforce.set_epsilon_to_number()\n", + "\n", + " For more information on use of epsilon to prevent singularities, take a look at the\n", + " Epsilon Tutorial: https://symforce.org/tutorials/epsilon_tutorial.html\n", + "\n" + ] + }, + { + "data": { + "text/html": [ + "
// -----------------------------------------------------------------------------\n",
+       "// This file was autogenerated by symforce from template:\n",
+       "//     function/FUNCTION.h.jinja\n",
+       "// Do NOT modify by hand.\n",
+       "// -----------------------------------------------------------------------------\n",
+       "\n",
+       "#pragma once\n",
+       "\n",
+       "#include <Eigen/Dense>\n",
+       "\n",
+       "namespace sym {\n",
+       "\n",
+       "/**\n",
+       " * This function was autogenerated from a symbolic function. Do not modify by hand.\n",
+       " *\n",
+       " * Symbolic function: compute_measurement\n",
+       " *\n",
+       " * Args:\n",
+       " *     state: Matrix28_1\n",
+       " *     epsilon: Scalar\n",
+       " *\n",
+       " * Outputs:\n",
+       " *     res: Matrix15_1\n",
+       " *     res_D_state: (15x28) jacobian of res (15) wrt arg state (28)\n",
+       " */\n",
+       "template <typename Scalar>\n",
+       "Eigen::Matrix<Scalar, 15, 1> ComputeMeasurementWithJacobian0(\n",
+       "    const Eigen::Matrix<Scalar, 28, 1>& state, const Scalar epsilon,\n",
+       "    Eigen::Matrix<Scalar, 15, 28>* const res_D_state = nullptr) {\n",
+       "  // Total ops: 339\n",
+       "\n",
+       "  // Unused inputs\n",
+       "  (void)epsilon;\n",
+       "\n",
+       "  // Input arrays\n",
+       "\n",
+       "  // Intermediate terms (99)\n",
+       "  const Scalar _tmp0 = 2 * state(1, 0);\n",
+       "  const Scalar _tmp1 = _tmp0 * state(0, 0);\n",
+       "  const Scalar _tmp2 = 2 * state(2, 0);\n",
+       "  const Scalar _tmp3 = _tmp2 * state(3, 0);\n",
+       "  const Scalar _tmp4 = _tmp1 + _tmp3;\n",
+       "  const Scalar _tmp5 = _tmp2 * state(0, 0);\n",
+       "  const Scalar _tmp6 = _tmp0 * state(3, 0);\n",
+       "  const Scalar _tmp7 = _tmp5 - _tmp6;\n",
+       "  const Scalar _tmp8 = 2 * std::pow(state(1, 0), Scalar(2));\n",
+       "  const Scalar _tmp9 = 2 * std::pow(state(2, 0), Scalar(2)) - 1;\n",
+       "  const Scalar _tmp10 = _tmp8 + _tmp9;\n",
+       "  const Scalar _tmp11 = -_tmp10;\n",
+       "  const Scalar _tmp12 = _tmp1 - _tmp3;\n",
+       "  const Scalar _tmp13 = 2 * state(3, 0);\n",
+       "  const Scalar _tmp14 = _tmp13 * state(0, 0);\n",
+       "  const Scalar _tmp15 = _tmp0 * state(2, 0);\n",
+       "  const Scalar _tmp16 = _tmp14 + _tmp15;\n",
+       "  const Scalar _tmp17 = 2 * std::pow(state(0, 0), Scalar(2));\n",
+       "  const Scalar _tmp18 = _tmp17 + _tmp9;\n",
+       "  const Scalar _tmp19 = -_tmp18;\n",
+       "  const Scalar _tmp20 = _tmp5 + _tmp6;\n",
+       "  const Scalar _tmp21 = _tmp14 - _tmp15;\n",
+       "  const Scalar _tmp22 = -_tmp21;\n",
+       "  const Scalar _tmp23 = _tmp17 + _tmp8 - 1;\n",
+       "  const Scalar _tmp24 = -_tmp23;\n",
+       "  const Scalar _tmp25 = -state(5, 0);\n",
+       "  const Scalar _tmp26 = _tmp25 + state(17, 0);\n",
+       "  const Scalar _tmp27 = -state(6, 0);\n",
+       "  const Scalar _tmp28 = _tmp27 + state(18, 0);\n",
+       "  const Scalar _tmp29 = -state(4, 0);\n",
+       "  const Scalar _tmp30 = _tmp29 + state(16, 0);\n",
+       "  const Scalar _tmp31 = _tmp25 + state(20, 0);\n",
+       "  const Scalar _tmp32 = _tmp27 + state(21, 0);\n",
+       "  const Scalar _tmp33 = _tmp29 + state(19, 0);\n",
+       "  const Scalar _tmp34 = _tmp25 + state(23, 0);\n",
+       "  const Scalar _tmp35 = _tmp27 + state(24, 0);\n",
+       "  const Scalar _tmp36 = _tmp29 + state(22, 0);\n",
+       "  const Scalar _tmp37 = _tmp25 + state(26, 0);\n",
+       "  const Scalar _tmp38 = _tmp27 + state(27, 0);\n",
+       "  const Scalar _tmp39 = _tmp29 + state(25, 0);\n",
+       "  const Scalar _tmp40 = _tmp0 * state(8, 0);\n",
+       "  const Scalar _tmp41 = _tmp2 * state(9, 0);\n",
+       "  const Scalar _tmp42 = 4 * state(0, 0);\n",
+       "  const Scalar _tmp43 = _tmp13 * state(9, 0);\n",
+       "  const Scalar _tmp44 = -_tmp2 * state(7, 0);\n",
+       "  const Scalar _tmp45 = _tmp13 * state(8, 0);\n",
+       "  const Scalar _tmp46 = _tmp0 * _tmp26;\n",
+       "  const Scalar _tmp47 = _tmp2 * _tmp28;\n",
+       "  const Scalar _tmp48 = _tmp13 * _tmp28;\n",
+       "  const Scalar _tmp49 = -_tmp2 * _tmp30;\n",
+       "  const Scalar _tmp50 = _tmp13 * _tmp26;\n",
+       "  const Scalar _tmp51 = _tmp0 * _tmp31;\n",
+       "  const Scalar _tmp52 = _tmp2 * _tmp32;\n",
+       "  const Scalar _tmp53 = _tmp13 * _tmp32;\n",
+       "  const Scalar _tmp54 = -_tmp2 * _tmp33;\n",
+       "  const Scalar _tmp55 = _tmp13 * _tmp31;\n",
+       "  const Scalar _tmp56 = _tmp0 * _tmp34;\n",
+       "  const Scalar _tmp57 = _tmp2 * _tmp35;\n",
+       "  const Scalar _tmp58 = _tmp13 * _tmp35;\n",
+       "  const Scalar _tmp59 = -_tmp2 * _tmp36;\n",
+       "  const Scalar _tmp60 = _tmp13 * _tmp34;\n",
+       "  const Scalar _tmp61 = _tmp0 * _tmp37;\n",
+       "  const Scalar _tmp62 = _tmp2 * _tmp38;\n",
+       "  const Scalar _tmp63 = _tmp13 * _tmp38;\n",
+       "  const Scalar _tmp64 = -_tmp2 * _tmp39;\n",
+       "  const Scalar _tmp65 = _tmp13 * _tmp37;\n",
+       "  const Scalar _tmp66 = 2 * state(0, 0);\n",
+       "  const Scalar _tmp67 = _tmp66 * state(8, 0);\n",
+       "  const Scalar _tmp68 = 4 * state(1, 0);\n",
+       "  const Scalar _tmp69 = _tmp66 * state(7, 0);\n",
+       "  const Scalar _tmp70 = _tmp13 * state(7, 0);\n",
+       "  const Scalar _tmp71 = _tmp26 * _tmp66;\n",
+       "  const Scalar _tmp72 = _tmp30 * _tmp66;\n",
+       "  const Scalar _tmp73 = _tmp13 * _tmp30;\n",
+       "  const Scalar _tmp74 = _tmp31 * _tmp66;\n",
+       "  const Scalar _tmp75 = _tmp33 * _tmp66;\n",
+       "  const Scalar _tmp76 = _tmp13 * _tmp33;\n",
+       "  const Scalar _tmp77 = _tmp34 * _tmp66;\n",
+       "  const Scalar _tmp78 = _tmp36 * _tmp66;\n",
+       "  const Scalar _tmp79 = _tmp13 * _tmp36;\n",
+       "  const Scalar _tmp80 = _tmp37 * _tmp66;\n",
+       "  const Scalar _tmp81 = _tmp39 * _tmp66;\n",
+       "  const Scalar _tmp82 = _tmp13 * _tmp39;\n",
+       "  const Scalar _tmp83 = _tmp66 * state(9, 0);\n",
+       "  const Scalar _tmp84 = 4 * state(2, 0);\n",
+       "  const Scalar _tmp85 = _tmp0 * state(9, 0);\n",
+       "  const Scalar _tmp86 = _tmp28 * _tmp66;\n",
+       "  const Scalar _tmp87 = _tmp0 * _tmp28;\n",
+       "  const Scalar _tmp88 = _tmp32 * _tmp66;\n",
+       "  const Scalar _tmp89 = _tmp0 * _tmp32;\n",
+       "  const Scalar _tmp90 = _tmp35 * _tmp66;\n",
+       "  const Scalar _tmp91 = _tmp0 * _tmp35;\n",
+       "  const Scalar _tmp92 = _tmp38 * _tmp66;\n",
+       "  const Scalar _tmp93 = _tmp0 * _tmp38;\n",
+       "  const Scalar _tmp94 = -_tmp12;\n",
+       "  const Scalar _tmp95 = -_tmp20;\n",
+       "  const Scalar _tmp96 = -_tmp4;\n",
+       "  const Scalar _tmp97 = -_tmp7;\n",
+       "  const Scalar _tmp98 = -_tmp16;\n",
+       "\n",
+       "  // Output terms (2)\n",
+       "  Eigen::Matrix<Scalar, 15, 1> _res;\n",
+       "\n",
+       "  _res(0, 0) = _tmp11 * state(7, 0) + _tmp4 * state(8, 0) + _tmp7 * state(9, 0);\n",
+       "  _res(1, 0) = _tmp12 * state(7, 0) + _tmp16 * state(9, 0) + _tmp19 * state(8, 0);\n",
+       "  _res(2, 0) = _tmp20 * state(7, 0) + _tmp22 * state(8, 0) + _tmp24 * state(9, 0);\n",
+       "  _res(3, 0) = _tmp11 * _tmp30 + _tmp26 * _tmp4 + _tmp28 * _tmp7;\n",
+       "  _res(4, 0) = _tmp12 * _tmp30 + _tmp16 * _tmp28 + _tmp19 * _tmp26;\n",
+       "  _res(5, 0) = _tmp20 * _tmp30 + _tmp22 * _tmp26 + _tmp24 * _tmp28;\n",
+       "  _res(6, 0) = _tmp11 * _tmp33 + _tmp31 * _tmp4 + _tmp32 * _tmp7;\n",
+       "  _res(7, 0) = _tmp12 * _tmp33 + _tmp16 * _tmp32 + _tmp19 * _tmp31;\n",
+       "  _res(8, 0) = _tmp20 * _tmp33 + _tmp22 * _tmp31 + _tmp24 * _tmp32;\n",
+       "  _res(9, 0) = _tmp11 * _tmp36 + _tmp34 * _tmp4 + _tmp35 * _tmp7;\n",
+       "  _res(10, 0) = _tmp12 * _tmp36 + _tmp16 * _tmp35 + _tmp19 * _tmp34;\n",
+       "  _res(11, 0) = _tmp20 * _tmp36 + _tmp22 * _tmp34 + _tmp24 * _tmp35;\n",
+       "  _res(12, 0) = _tmp11 * _tmp39 + _tmp37 * _tmp4 + _tmp38 * _tmp7;\n",
+       "  _res(13, 0) = _tmp12 * _tmp39 + _tmp16 * _tmp38 + _tmp19 * _tmp37;\n",
+       "  _res(14, 0) = _tmp20 * _tmp39 + _tmp22 * _tmp37 + _tmp24 * _tmp38;\n",
+       "\n",
+       "  if (res_D_state != nullptr) {\n",
+       "    Eigen::Matrix<Scalar, 15, 28>& _res_D_state = (*res_D_state);\n",
+       "\n",
+       "    _res_D_state.setZero();\n",
+       "\n",
+       "    _res_D_state(0, 0) = _tmp40 + _tmp41;\n",
+       "    _res_D_state(1, 0) = _tmp0 * state(7, 0) - _tmp42 * state(8, 0) + _tmp43;\n",
+       "    _res_D_state(2, 0) = -_tmp42 * state(9, 0) - _tmp44 - _tmp45;\n",
+       "    _res_D_state(3, 0) = _tmp46 + _tmp47;\n",
+       "    _res_D_state(4, 0) = _tmp0 * _tmp30 - _tmp26 * _tmp42 + _tmp48;\n",
+       "    _res_D_state(5, 0) = -_tmp28 * _tmp42 - _tmp49 - _tmp50;\n",
+       "    _res_D_state(6, 0) = _tmp51 + _tmp52;\n",
+       "    _res_D_state(7, 0) = _tmp0 * _tmp33 - _tmp31 * _tmp42 + _tmp53;\n",
+       "    _res_D_state(8, 0) = -_tmp32 * _tmp42 - _tmp54 - _tmp55;\n",
+       "    _res_D_state(9, 0) = _tmp56 + _tmp57;\n",
+       "    _res_D_state(10, 0) = _tmp0 * _tmp36 - _tmp34 * _tmp42 + _tmp58;\n",
+       "    _res_D_state(11, 0) = -_tmp35 * _tmp42 - _tmp59 - _tmp60;\n",
+       "    _res_D_state(12, 0) = _tmp61 + _tmp62;\n",
+       "    _res_D_state(13, 0) = _tmp0 * _tmp39 - _tmp37 * _tmp42 + _tmp63;\n",
+       "    _res_D_state(14, 0) = -_tmp38 * _tmp42 - _tmp64 - _tmp65;\n",
+       "    _res_D_state(0, 1) = -_tmp43 + _tmp67 - _tmp68 * state(7, 0);\n",
+       "    _res_D_state(1, 1) = _tmp41 + _tmp69;\n",
+       "    _res_D_state(2, 1) = _tmp2 * state(8, 0) - _tmp68 * state(9, 0) + _tmp70;\n",
+       "    _res_D_state(3, 1) = -_tmp30 * _tmp68 - _tmp48 + _tmp71;\n",
+       "    _res_D_state(4, 1) = _tmp47 + _tmp72;\n",
+       "    _res_D_state(5, 1) = _tmp2 * _tmp26 - _tmp28 * _tmp68 + _tmp73;\n",
+       "    _res_D_state(6, 1) = -_tmp33 * _tmp68 - _tmp53 + _tmp74;\n",
+       "    _res_D_state(7, 1) = _tmp52 + _tmp75;\n",
+       "    _res_D_state(8, 1) = _tmp2 * _tmp31 - _tmp32 * _tmp68 + _tmp76;\n",
+       "    _res_D_state(9, 1) = -_tmp36 * _tmp68 - _tmp58 + _tmp77;\n",
+       "    _res_D_state(10, 1) = _tmp57 + _tmp78;\n",
+       "    _res_D_state(11, 1) = _tmp2 * _tmp34 - _tmp35 * _tmp68 + _tmp79;\n",
+       "    _res_D_state(12, 1) = -_tmp39 * _tmp68 - _tmp63 + _tmp80;\n",
+       "    _res_D_state(13, 1) = _tmp62 + _tmp81;\n",
+       "    _res_D_state(14, 1) = _tmp2 * _tmp37 - _tmp38 * _tmp68 + _tmp82;\n",
+       "    _res_D_state(0, 2) = _tmp45 + _tmp83 - _tmp84 * state(7, 0);\n",
+       "    _res_D_state(1, 2) = -_tmp70 - _tmp84 * state(8, 0) + _tmp85;\n",
+       "    _res_D_state(2, 2) = _tmp40 + _tmp69;\n",
+       "    _res_D_state(3, 2) = -_tmp30 * _tmp84 + _tmp50 + _tmp86;\n",
+       "    _res_D_state(4, 2) = -_tmp26 * _tmp84 - _tmp73 + _tmp87;\n",
+       "    _res_D_state(5, 2) = _tmp46 + _tmp72;\n",
+       "    _res_D_state(6, 2) = -_tmp33 * _tmp84 + _tmp55 + _tmp88;\n",
+       "    _res_D_state(7, 2) = -_tmp31 * _tmp84 - _tmp76 + _tmp89;\n",
+       "    _res_D_state(8, 2) = _tmp51 + _tmp75;\n",
+       "    _res_D_state(9, 2) = -_tmp36 * _tmp84 + _tmp60 + _tmp90;\n",
+       "    _res_D_state(10, 2) = -_tmp34 * _tmp84 - _tmp79 + _tmp91;\n",
+       "    _res_D_state(11, 2) = _tmp56 + _tmp78;\n",
+       "    _res_D_state(12, 2) = -_tmp39 * _tmp84 + _tmp65 + _tmp92;\n",
+       "    _res_D_state(13, 2) = -_tmp37 * _tmp84 - _tmp82 + _tmp93;\n",
+       "    _res_D_state(14, 2) = _tmp61 + _tmp81;\n",
+       "    _res_D_state(0, 3) = -_tmp85 + 2 * state(2, 0) * state(8, 0);\n",
+       "    _res_D_state(1, 3) = _tmp44 + _tmp83;\n",
+       "    _res_D_state(2, 3) = -_tmp67 + 2 * state(1, 0) * state(7, 0);\n",
+       "    _res_D_state(3, 3) = 2 * _tmp26 * state(2, 0) - _tmp87;\n",
+       "    _res_D_state(4, 3) = _tmp49 + _tmp86;\n",
+       "    _res_D_state(5, 3) = 2 * _tmp30 * state(1, 0) - _tmp71;\n",
+       "    _res_D_state(6, 3) = 2 * _tmp31 * state(2, 0) - _tmp89;\n",
+       "    _res_D_state(7, 3) = _tmp54 + _tmp88;\n",
+       "    _res_D_state(8, 3) = 2 * _tmp33 * state(1, 0) - _tmp74;\n",
+       "    _res_D_state(9, 3) = 2 * _tmp34 * state(2, 0) - _tmp91;\n",
+       "    _res_D_state(10, 3) = _tmp59 + _tmp90;\n",
+       "    _res_D_state(11, 3) = 2 * _tmp36 * state(1, 0) - _tmp77;\n",
+       "    _res_D_state(12, 3) = 2 * _tmp37 * state(2, 0) - _tmp93;\n",
+       "    _res_D_state(13, 3) = _tmp64 + _tmp92;\n",
+       "    _res_D_state(14, 3) = 2 * _tmp39 * state(1, 0) - _tmp80;\n",
+       "    _res_D_state(3, 4) = _tmp10;\n",
+       "    _res_D_state(4, 4) = _tmp94;\n",
+       "    _res_D_state(5, 4) = _tmp95;\n",
+       "    _res_D_state(6, 4) = _tmp10;\n",
+       "    _res_D_state(7, 4) = _tmp94;\n",
+       "    _res_D_state(8, 4) = _tmp95;\n",
+       "    _res_D_state(9, 4) = _tmp10;\n",
+       "    _res_D_state(10, 4) = _tmp94;\n",
+       "    _res_D_state(11, 4) = _tmp95;\n",
+       "    _res_D_state(12, 4) = _tmp10;\n",
+       "    _res_D_state(13, 4) = _tmp94;\n",
+       "    _res_D_state(14, 4) = _tmp95;\n",
+       "    _res_D_state(3, 5) = _tmp96;\n",
+       "    _res_D_state(4, 5) = _tmp18;\n",
+       "    _res_D_state(5, 5) = _tmp21;\n",
+       "    _res_D_state(6, 5) = _tmp96;\n",
+       "    _res_D_state(7, 5) = _tmp18;\n",
+       "    _res_D_state(8, 5) = _tmp21;\n",
+       "    _res_D_state(9, 5) = _tmp96;\n",
+       "    _res_D_state(10, 5) = _tmp18;\n",
+       "    _res_D_state(11, 5) = _tmp21;\n",
+       "    _res_D_state(12, 5) = _tmp96;\n",
+       "    _res_D_state(13, 5) = _tmp18;\n",
+       "    _res_D_state(14, 5) = _tmp21;\n",
+       "    _res_D_state(3, 6) = _tmp97;\n",
+       "    _res_D_state(4, 6) = _tmp98;\n",
+       "    _res_D_state(5, 6) = _tmp23;\n",
+       "    _res_D_state(6, 6) = _tmp97;\n",
+       "    _res_D_state(7, 6) = _tmp98;\n",
+       "    _res_D_state(8, 6) = _tmp23;\n",
+       "    _res_D_state(9, 6) = _tmp97;\n",
+       "    _res_D_state(10, 6) = _tmp98;\n",
+       "    _res_D_state(11, 6) = _tmp23;\n",
+       "    _res_D_state(12, 6) = _tmp97;\n",
+       "    _res_D_state(13, 6) = _tmp98;\n",
+       "    _res_D_state(14, 6) = _tmp23;\n",
+       "    _res_D_state(0, 7) = _tmp11;\n",
+       "    _res_D_state(1, 7) = _tmp12;\n",
+       "    _res_D_state(2, 7) = _tmp20;\n",
+       "    _res_D_state(0, 8) = _tmp4;\n",
+       "    _res_D_state(1, 8) = _tmp19;\n",
+       "    _res_D_state(2, 8) = _tmp22;\n",
+       "    _res_D_state(0, 9) = _tmp7;\n",
+       "    _res_D_state(1, 9) = _tmp16;\n",
+       "    _res_D_state(2, 9) = _tmp24;\n",
+       "    _res_D_state(3, 16) = _tmp11;\n",
+       "    _res_D_state(4, 16) = _tmp12;\n",
+       "    _res_D_state(5, 16) = _tmp20;\n",
+       "    _res_D_state(3, 17) = _tmp4;\n",
+       "    _res_D_state(4, 17) = _tmp19;\n",
+       "    _res_D_state(5, 17) = _tmp22;\n",
+       "    _res_D_state(3, 18) = _tmp7;\n",
+       "    _res_D_state(4, 18) = _tmp16;\n",
+       "    _res_D_state(5, 18) = _tmp24;\n",
+       "    _res_D_state(6, 19) = _tmp11;\n",
+       "    _res_D_state(7, 19) = _tmp12;\n",
+       "    _res_D_state(8, 19) = _tmp20;\n",
+       "    _res_D_state(6, 20) = _tmp4;\n",
+       "    _res_D_state(7, 20) = _tmp19;\n",
+       "    _res_D_state(8, 20) = _tmp22;\n",
+       "    _res_D_state(6, 21) = _tmp7;\n",
+       "    _res_D_state(7, 21) = _tmp16;\n",
+       "    _res_D_state(8, 21) = _tmp24;\n",
+       "    _res_D_state(9, 22) = _tmp11;\n",
+       "    _res_D_state(10, 22) = _tmp12;\n",
+       "    _res_D_state(11, 22) = _tmp20;\n",
+       "    _res_D_state(9, 23) = _tmp4;\n",
+       "    _res_D_state(10, 23) = _tmp19;\n",
+       "    _res_D_state(11, 23) = _tmp22;\n",
+       "    _res_D_state(9, 24) = _tmp7;\n",
+       "    _res_D_state(10, 24) = _tmp16;\n",
+       "    _res_D_state(11, 24) = _tmp24;\n",
+       "    _res_D_state(12, 25) = _tmp11;\n",
+       "    _res_D_state(13, 25) = _tmp12;\n",
+       "    _res_D_state(14, 25) = _tmp20;\n",
+       "    _res_D_state(12, 26) = _tmp4;\n",
+       "    _res_D_state(13, 26) = _tmp19;\n",
+       "    _res_D_state(14, 26) = _tmp22;\n",
+       "    _res_D_state(12, 27) = _tmp7;\n",
+       "    _res_D_state(13, 27) = _tmp16;\n",
+       "    _res_D_state(14, 27) = _tmp24;\n",
+       "  }\n",
+       "\n",
+       "  return _res;\n",
+       "}  // NOLINT(readability/fn_size)\n",
+       "\n",
+       "// NOLINTNEXTLINE(readability/fn_size)\n",
+       "}  // namespace sym\n",
+       "
\n" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "compute_measurement_codegen = codegen.Codegen.function(\n", + " func=compute_measurement,\n", + " config=codegen.CppConfig(),\n", + ")\n", + "\n", + "codegen_with_jacobians = compute_measurement_codegen.with_jacobians(\n", + " # Just compute wrt the pose and point, not epsilon\n", + " which_args=[\"state\"],\n", + " # Include value, not just jacobians\n", + " include_results=True,\n", + ")\n", + "\n", + "data = codegen_with_jacobians.generate_function()\n", + "\n", + "display_code_file(data.generated_files[0], \"C++\")\n" + ] + }, + { + "cell_type": "code", + "execution_count": 22, + "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "codegen.__init__():141 WARNING -- \n", + " Generating code with epsilon set to 0 - This is dangerous! You may get NaNs, Infs,\n", + " or numerically unstable results from calling generated functions near singularities.\n", + "\n", + " In order to safely generate code, you should set epsilon to either a symbol\n", + " (recommended) or a small numerical value like `sf.numeric_epsilon`. You should do\n", + " this before importing any other code from symforce, e.g. with\n", + "\n", + " import symforce\n", + " symforce.set_epsilon_to_symbol()\n", + "\n", + " or\n", + "\n", + " import symforce\n", + " symforce.set_epsilon_to_number()\n", + "\n", + " For more information on use of epsilon to prevent singularities, take a look at the\n", + " Epsilon Tutorial: https://symforce.org/tutorials/epsilon_tutorial.html\n", + "\n", + "codegen.__init__():141 WARNING -- \n", + " Generating code with epsilon set to 0 - This is dangerous! You may get NaNs, Infs,\n", + " or numerically unstable results from calling generated functions near singularities.\n", + "\n", + " In order to safely generate code, you should set epsilon to either a symbol\n", + " (recommended) or a small numerical value like `sf.numeric_epsilon`. You should do\n", + " this before importing any other code from symforce, e.g. with\n", + "\n", + " import symforce\n", + " symforce.set_epsilon_to_symbol()\n", + "\n", + " or\n", + "\n", + " import symforce\n", + " symforce.set_epsilon_to_number()\n", + "\n", + " For more information on use of epsilon to prevent singularities, take a look at the\n", + " Epsilon Tutorial: https://symforce.org/tutorials/epsilon_tutorial.html\n", + "\n" + ] + }, + { + "data": { + "text/html": [ + "
// -----------------------------------------------------------------------------\n",
+       "// This file was autogenerated by symforce from template:\n",
+       "//     function/FUNCTION.h.jinja\n",
+       "// Do NOT modify by hand.\n",
+       "// -----------------------------------------------------------------------------\n",
+       "\n",
+       "#pragma once\n",
+       "\n",
+       "#include <Eigen/Dense>\n",
+       "\n",
+       "namespace sym {\n",
+       "\n",
+       "/**\n",
+       " * This function was autogenerated from a symbolic function. Do not modify by hand.\n",
+       " *\n",
+       " * Symbolic function: compute_mean\n",
+       " *\n",
+       " * Args:\n",
+       " *     state: Matrix28_1\n",
+       " *     u: Matrix61\n",
+       " *     dT: Scalar\n",
+       " *     epsilon: Scalar\n",
+       " *\n",
+       " * Outputs:\n",
+       " *     res: Matrix28_1\n",
+       " *     res_D_state: (28x28) jacobian of res (28) wrt arg state (28)\n",
+       " */\n",
+       "template <typename Scalar>\n",
+       "Eigen::Matrix<Scalar, 28, 1> ComputeMeanWithJacobian0(\n",
+       "    const Eigen::Matrix<Scalar, 28, 1>& state, const Eigen::Matrix<Scalar, 6, 1>& u,\n",
+       "    const Scalar dT, const Scalar epsilon,\n",
+       "    Eigen::Matrix<Scalar, 28, 28>* const res_D_state = nullptr) {\n",
+       "  // Total ops: 150\n",
+       "\n",
+       "  // Unused inputs\n",
+       "  (void)epsilon;\n",
+       "\n",
+       "  // Input arrays\n",
+       "\n",
+       "  // Intermediate terms (49)\n",
+       "  const Scalar _tmp0 = dT * (-state(12, 0) + u(2, 0));\n",
+       "  const Scalar _tmp1 = dT * (-state(10, 0) + u(0, 0));\n",
+       "  const Scalar _tmp2 = dT * (-state(11, 0) + u(1, 0));\n",
+       "  const Scalar _tmp3 = -state(14, 0) + u(4, 0);\n",
+       "  const Scalar _tmp4 = 2 * state(1, 0);\n",
+       "  const Scalar _tmp5 = _tmp4 * state(0, 0);\n",
+       "  const Scalar _tmp6 = 2 * state(2, 0);\n",
+       "  const Scalar _tmp7 = _tmp6 * state(3, 0);\n",
+       "  const Scalar _tmp8 = _tmp5 - _tmp7;\n",
+       "  const Scalar _tmp9 = -state(15, 0) + u(5, 0);\n",
+       "  const Scalar _tmp10 = _tmp6 * state(0, 0);\n",
+       "  const Scalar _tmp11 = _tmp4 * state(3, 0);\n",
+       "  const Scalar _tmp12 = _tmp10 + _tmp11;\n",
+       "  const Scalar _tmp13 = -state(13, 0) + u(3, 0);\n",
+       "  const Scalar _tmp14 = 2 * std::pow(state(1, 0), Scalar(2));\n",
+       "  const Scalar _tmp15 = 2 * std::pow(state(2, 0), Scalar(2)) - 1;\n",
+       "  const Scalar _tmp16 = _tmp14 + _tmp15;\n",
+       "  const Scalar _tmp17 = _tmp5 + _tmp7;\n",
+       "  const Scalar _tmp18 = 2 * state(3, 0);\n",
+       "  const Scalar _tmp19 = _tmp18 * state(0, 0);\n",
+       "  const Scalar _tmp20 = _tmp6 * state(1, 0);\n",
+       "  const Scalar _tmp21 = _tmp19 - _tmp20;\n",
+       "  const Scalar _tmp22 = 2 * std::pow(state(0, 0), Scalar(2));\n",
+       "  const Scalar _tmp23 = _tmp15 + _tmp22;\n",
+       "  const Scalar _tmp24 = _tmp10 - _tmp11;\n",
+       "  const Scalar _tmp25 = _tmp19 + _tmp20;\n",
+       "  const Scalar _tmp26 = _tmp14 + _tmp22 - 1;\n",
+       "  const Scalar _tmp27 = -_tmp0;\n",
+       "  const Scalar _tmp28 = -_tmp1;\n",
+       "  const Scalar _tmp29 = _tmp3 * _tmp4;\n",
+       "  const Scalar _tmp30 = _tmp6 * _tmp9;\n",
+       "  const Scalar _tmp31 = 4 * state(0, 0);\n",
+       "  const Scalar _tmp32 = -_tmp13 * _tmp4;\n",
+       "  const Scalar _tmp33 = _tmp18 * _tmp9;\n",
+       "  const Scalar _tmp34 = _tmp18 * _tmp3;\n",
+       "  const Scalar _tmp35 = -_tmp2;\n",
+       "  const Scalar _tmp36 = 2 * state(0, 0);\n",
+       "  const Scalar _tmp37 = _tmp3 * _tmp36;\n",
+       "  const Scalar _tmp38 = 4 * state(1, 0);\n",
+       "  const Scalar _tmp39 = _tmp13 * _tmp36;\n",
+       "  const Scalar _tmp40 = -_tmp3 * _tmp6;\n",
+       "  const Scalar _tmp41 = _tmp13 * _tmp18;\n",
+       "  const Scalar _tmp42 = _tmp36 * _tmp9;\n",
+       "  const Scalar _tmp43 = 4 * state(2, 0);\n",
+       "  const Scalar _tmp44 = _tmp4 * _tmp9;\n",
+       "  const Scalar _tmp45 = -dT * state(3, 0);\n",
+       "  const Scalar _tmp46 = dT * state(2, 0);\n",
+       "  const Scalar _tmp47 = dT * state(1, 0);\n",
+       "  const Scalar _tmp48 = dT * state(0, 0);\n",
+       "\n",
+       "  // Output terms (2)\n",
+       "  Eigen::Matrix<Scalar, 28, 1> _res;\n",
+       "\n",
+       "  _res(0, 0) =\n",
+       "      _tmp0 * state(1, 0) + _tmp1 * state(3, 0) - _tmp2 * state(2, 0) + Scalar(1.0) * state(0, 0);\n",
+       "  _res(1, 0) =\n",
+       "      -_tmp0 * state(0, 0) + _tmp1 * state(2, 0) + _tmp2 * state(3, 0) + Scalar(1.0) * state(1, 0);\n",
+       "  _res(2, 0) =\n",
+       "      _tmp0 * state(3, 0) - _tmp1 * state(1, 0) + _tmp2 * state(0, 0) + Scalar(1.0) * state(2, 0);\n",
+       "  _res(3, 0) =\n",
+       "      -_tmp0 * state(2, 0) - _tmp1 * state(0, 0) - _tmp2 * state(1, 0) + Scalar(1.0) * state(3, 0);\n",
+       "  _res(4, 0) = dT * state(7, 0) + state(4, 0);\n",
+       "  _res(5, 0) = dT * state(8, 0) + state(5, 0);\n",
+       "  _res(6, 0) = dT * state(9, 0) + state(6, 0);\n",
+       "  _res(7, 0) = _tmp12 * _tmp9 - _tmp13 * _tmp16 + _tmp3 * _tmp8 + state(7, 0);\n",
+       "  _res(8, 0) = _tmp13 * _tmp17 - _tmp21 * _tmp9 - _tmp23 * _tmp3 + state(8, 0);\n",
+       "  _res(9, 0) =\n",
+       "      _tmp13 * _tmp24 + _tmp25 * _tmp3 - _tmp26 * _tmp9 + state(9, 0) + Scalar(9.8000000000000007);\n",
+       "  _res(10, 0) = state(10, 0);\n",
+       "  _res(11, 0) = state(11, 0);\n",
+       "  _res(12, 0) = state(12, 0);\n",
+       "  _res(13, 0) = state(13, 0);\n",
+       "  _res(14, 0) = state(14, 0);\n",
+       "  _res(15, 0) = state(15, 0);\n",
+       "  _res(16, 0) = state(16, 0);\n",
+       "  _res(17, 0) = state(17, 0);\n",
+       "  _res(18, 0) = state(18, 0);\n",
+       "  _res(19, 0) = state(19, 0);\n",
+       "  _res(20, 0) = state(20, 0);\n",
+       "  _res(21, 0) = state(21, 0);\n",
+       "  _res(22, 0) = state(22, 0);\n",
+       "  _res(23, 0) = state(23, 0);\n",
+       "  _res(24, 0) = state(24, 0);\n",
+       "  _res(25, 0) = state(25, 0);\n",
+       "  _res(26, 0) = state(26, 0);\n",
+       "  _res(27, 0) = state(27, 0);\n",
+       "\n",
+       "  if (res_D_state != nullptr) {\n",
+       "    Eigen::Matrix<Scalar, 28, 28>& _res_D_state = (*res_D_state);\n",
+       "\n",
+       "    _res_D_state.setZero();\n",
+       "\n",
+       "    _res_D_state(0, 0) = Scalar(1.0);\n",
+       "    _res_D_state(1, 0) = _tmp27;\n",
+       "    _res_D_state(2, 0) = _tmp2;\n",
+       "    _res_D_state(3, 0) = _tmp28;\n",
+       "    _res_D_state(7, 0) = _tmp29 + _tmp30;\n",
+       "    _res_D_state(8, 0) = -_tmp3 * _tmp31 - _tmp32 - _tmp33;\n",
+       "    _res_D_state(9, 0) = _tmp13 * _tmp6 - _tmp31 * _tmp9 + _tmp34;\n",
+       "    _res_D_state(0, 1) = _tmp0;\n",
+       "    _res_D_state(1, 1) = Scalar(1.0);\n",
+       "    _res_D_state(2, 1) = _tmp28;\n",
+       "    _res_D_state(3, 1) = _tmp35;\n",
+       "    _res_D_state(7, 1) = -_tmp13 * _tmp38 + _tmp33 + _tmp37;\n",
+       "    _res_D_state(8, 1) = _tmp30 + _tmp39;\n",
+       "    _res_D_state(9, 1) = -_tmp38 * _tmp9 - _tmp40 - _tmp41;\n",
+       "    _res_D_state(0, 2) = _tmp35;\n",
+       "    _res_D_state(1, 2) = _tmp1;\n",
+       "    _res_D_state(2, 2) = Scalar(1.0);\n",
+       "    _res_D_state(3, 2) = _tmp27;\n",
+       "    _res_D_state(7, 2) = -_tmp13 * _tmp43 - _tmp34 + _tmp42;\n",
+       "    _res_D_state(8, 2) = -_tmp3 * _tmp43 + _tmp41 + _tmp44;\n",
+       "    _res_D_state(9, 2) = _tmp29 + _tmp39;\n",
+       "    _res_D_state(0, 3) = _tmp1;\n",
+       "    _res_D_state(1, 3) = _tmp2;\n",
+       "    _res_D_state(2, 3) = _tmp0;\n",
+       "    _res_D_state(3, 3) = Scalar(1.0);\n",
+       "    _res_D_state(7, 3) = _tmp40 + _tmp44;\n",
+       "    _res_D_state(8, 3) = 2 * _tmp13 * state(2, 0) - _tmp42;\n",
+       "    _res_D_state(9, 3) = _tmp32 + _tmp37;\n",
+       "    _res_D_state(4, 4) = 1;\n",
+       "    _res_D_state(5, 5) = 1;\n",
+       "    _res_D_state(6, 6) = 1;\n",
+       "    _res_D_state(4, 7) = dT;\n",
+       "    _res_D_state(7, 7) = 1;\n",
+       "    _res_D_state(5, 8) = dT;\n",
+       "    _res_D_state(8, 8) = 1;\n",
+       "    _res_D_state(6, 9) = dT;\n",
+       "    _res_D_state(9, 9) = 1;\n",
+       "    _res_D_state(0, 10) = _tmp45;\n",
+       "    _res_D_state(1, 10) = -_tmp46;\n",
+       "    _res_D_state(2, 10) = _tmp47;\n",
+       "    _res_D_state(3, 10) = _tmp48;\n",
+       "    _res_D_state(10, 10) = 1;\n",
+       "    _res_D_state(0, 11) = _tmp46;\n",
+       "    _res_D_state(1, 11) = _tmp45;\n",
+       "    _res_D_state(2, 11) = -_tmp48;\n",
+       "    _res_D_state(3, 11) = _tmp47;\n",
+       "    _res_D_state(11, 11) = 1;\n",
+       "    _res_D_state(0, 12) = -_tmp47;\n",
+       "    _res_D_state(1, 12) = _tmp48;\n",
+       "    _res_D_state(2, 12) = _tmp45;\n",
+       "    _res_D_state(3, 12) = _tmp46;\n",
+       "    _res_D_state(12, 12) = 1;\n",
+       "    _res_D_state(7, 13) = _tmp16;\n",
+       "    _res_D_state(8, 13) = -_tmp17;\n",
+       "    _res_D_state(9, 13) = -_tmp24;\n",
+       "    _res_D_state(13, 13) = 1;\n",
+       "    _res_D_state(7, 14) = -_tmp8;\n",
+       "    _res_D_state(8, 14) = _tmp23;\n",
+       "    _res_D_state(9, 14) = -_tmp25;\n",
+       "    _res_D_state(14, 14) = 1;\n",
+       "    _res_D_state(7, 15) = -_tmp12;\n",
+       "    _res_D_state(8, 15) = _tmp21;\n",
+       "    _res_D_state(9, 15) = _tmp26;\n",
+       "    _res_D_state(15, 15) = 1;\n",
+       "    _res_D_state(16, 16) = 1;\n",
+       "    _res_D_state(17, 17) = 1;\n",
+       "    _res_D_state(18, 18) = 1;\n",
+       "    _res_D_state(19, 19) = 1;\n",
+       "    _res_D_state(20, 20) = 1;\n",
+       "    _res_D_state(21, 21) = 1;\n",
+       "    _res_D_state(22, 22) = 1;\n",
+       "    _res_D_state(23, 23) = 1;\n",
+       "    _res_D_state(24, 24) = 1;\n",
+       "    _res_D_state(25, 25) = 1;\n",
+       "    _res_D_state(26, 26) = 1;\n",
+       "    _res_D_state(27, 27) = 1;\n",
+       "  }\n",
+       "\n",
+       "  return _res;\n",
+       "}  // NOLINT(readability/fn_size)\n",
+       "\n",
+       "// NOLINTNEXTLINE(readability/fn_size)\n",
+       "}  // namespace sym\n",
+       "
\n" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "compute_mean_codegen = codegen.Codegen.function(\n", + " func=compute_mean,\n", + " config=codegen.CppConfig(),\n", + ")\n", + "\n", + "codegen_with_jacobians = compute_mean_codegen.with_jacobians(\n", + " # Just compute wrt the pose and point, not epsilon\n", + " which_args=[\"state\"],\n", + " # Include value, not just jacobians\n", + " include_results=True,\n", + ")\n", + "\n", + "data = codegen_with_jacobians.generate_function()\n", + "\n", + "display_code_file(data.generated_files[0], \"C++\")\n" + ] + }, + { + "cell_type": "code", + "execution_count": 26, + "metadata": {}, + "outputs": [ + { + "ename": "TypeError", + "evalue": "__init__() got an unexpected keyword argument 'func'", + "output_type": "error", + "traceback": [ + "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[0;31mTypeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[0;32mIn[26], line 2\u001b[0m\n\u001b[1;32m 1\u001b[0m namespace \u001b[38;5;241m=\u001b[39m \u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mdouble_pendulum\u001b[39m\u001b[38;5;124m\"\u001b[39m\n\u001b[0;32m----> 2\u001b[0m double_pendulum_python \u001b[38;5;241m=\u001b[39m \u001b[43mcodegen\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mCodegen\u001b[49m\u001b[43m(\u001b[49m\n\u001b[1;32m 3\u001b[0m \u001b[43m \u001b[49m\u001b[43mfunc\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[43mcompute_mean\u001b[49m\u001b[43m,\u001b[49m\n\u001b[1;32m 4\u001b[0m \u001b[43m \u001b[49m\u001b[43mconfig\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mcodegen\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mPythonConfig\u001b[49m\u001b[43m(\u001b[49m\u001b[43muse_eigen_types\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;28;43;01mFalse\u001b[39;49;00m\u001b[43m)\u001b[49m\u001b[43m,\u001b[49m\n\u001b[1;32m 5\u001b[0m \u001b[43m \u001b[49m\u001b[43mname\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;124;43m\"\u001b[39;49m\u001b[38;5;124;43mdouble_pendulum\u001b[39;49m\u001b[38;5;124;43m\"\u001b[39;49m\u001b[43m,\u001b[49m\n\u001b[1;32m 6\u001b[0m \u001b[43m \u001b[49m\u001b[43mreturn_key\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;124;43m\"\u001b[39;49m\u001b[38;5;124;43mddang\u001b[39;49m\u001b[38;5;124;43m\"\u001b[39;49m\u001b[43m,\u001b[49m\n\u001b[1;32m 7\u001b[0m \u001b[43m)\u001b[49m\n\u001b[1;32m 8\u001b[0m double_pendulum_python_data \u001b[38;5;241m=\u001b[39m double_pendulum_python\u001b[38;5;241m.\u001b[39mgenerate_function(\n\u001b[1;32m 9\u001b[0m namespace\u001b[38;5;241m=\u001b[39mnamespace,\n\u001b[1;32m 10\u001b[0m )\n\u001b[1;32m 12\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mFiles generated in \u001b[39m\u001b[38;5;132;01m{}\u001b[39;00m\u001b[38;5;124m:\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;241m.\u001b[39mformat(double_pendulum_python_data\u001b[38;5;241m.\u001b[39moutput_dir))\n", + "\u001b[0;31mTypeError\u001b[0m: __init__() got an unexpected keyword argument 'func'" + ] + } + ], + "source": [ + "namespace = \"double_pendulum\"\n", + "double_pendulum_python = codegen.Codegen.function(\n", + " func = compute_mean,\n", + " config=codegen.PythonConfig(use_eigen_types=False),\n", + " name=\"double_pendulum\",\n", + " return_key=\"ddang\",\n", + ")\n", + "double_pendulum_python_data = double_pendulum_python.generate_function(\n", + " namespace=namespace,\n", + ")\n", + "\n", + "print(\"Files generated in {}:\\n\".format(double_pendulum_python_data.output_dir))\n", + "for f in double_pendulum_python_data.generated_files:\n", + " print(\" |- {}\".format(f.relative_to(double_pendulum_python_data.output_dir)))\n", + "\n", + "display_code_file(\n", + " double_pendulum_python_data.function_dir / \"double_pendulum.py\",\n", + " \"python\",\n", + ")\n", + "\n", + "codegen_with_jacobians = compute_mean_codegen.with_jacobians(\n", + " # Just compute wrt the pose and point, not epsilon\n", + " which_args=[\"state\"],\n", + " # Include value, not just jacobians\n", + " include_results=True,\n", + ")" ] }, { @@ -226,22 +2810,7 @@ "execution_count": null, "metadata": {}, "outputs": [], - "source": [ - "def hat(w):\n", - " return np.array([[0., -w[2], w[1]],\n", - " [w[2], 0., -w[0]],\n", - " [-w[1], w[0], 0.]])" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "model.updateKinematicsPose(state['q'], np.eye(4))\n", - "J = model.getInfo()['Jb']" - ] + "source": [] } ], "metadata": { diff --git a/examples/walk_these_ways_sim.ipynb b/examples/walk_these_ways_sim.ipynb index 471fbc6..7a77f33 100644 --- a/examples/walk_these_ways_sim.ipynb +++ b/examples/walk_these_ways_sim.ipynb @@ -188,20 +188,6 @@ "text": [ "p_gains: [20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20.]\n" ] - }, - { - "name": "stdout", - "output_type": "stream", - "text": [ - "frq: 0.10237921921805228 Hz\n", - "frq: 46.14348108298404 Hz\n", - "frq: 44.397794032030994 Hz\n", - "frq: 45.603148715941465 Hz\n", - "frq: 45.86595514341641 Hz\n", - "frq: 44.16591026356524 Hz\n", - "frq: 45.82636627843454 Hz\n", - "frq: 45.51950772169346 Hz\n" - ] } ], "source": [ @@ -213,31 +199,9 @@ }, { "cell_type": "code", - "execution_count": 7, + "execution_count": 6, "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "frq: 45.741406386320016 Hz\n", - "frq: 44.6901430961184 Hz\n", - "frq: 44.451434445775085 Hz\n", - "frq: 45.19042385846963 Hz\n", - "frq: 45.1087737411542 Hz\n", - "frq: 46.690013692073066 Hz\n", - "frq: 45.09228519824546 Hz\n", - "frq: 45.70053825535531 Hz\n", - "frq: 46.01388873651992 Hz\n", - "frq: 44.94924554183813 Hz\n", - "frq: 45.49383372200228 Hz\n", - "frq: 44.522684330085134 Hz\n", - "frq: 44.42130458266699 Hz\n", - "frq: 43.71525649844704 Hz\n", - "frq: 44.37383889464886 Hz\n" - ] - } - ], + "outputs": [], "source": [ "controller.command_profile.pitch_cmd=0.0\n", "controller.command_profile.body_height_cmd=0.0\n",