From f34c3a73f6c44630fbf1a65fd8f3a1bf5507b608 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 28 Mar 2022 11:01:58 +0200
Subject: [PATCH] Refactor equations code into own files
---
.../cornelius_joint_mecha_demo.py | 300 +----
.../hemisphere-joint-demo/equations.py | 1057 +++++++++++++++++
.../hemisphere-joint-demo/terms.py | 13 +
3 files changed, 1091 insertions(+), 279 deletions(-)
create mode 100755 python/hemisphere-joint-demo/hemisphere-joint-demo/equations.py
create mode 100755 python/hemisphere-joint-demo/hemisphere-joint-demo/terms.py
diff --git a/python/hemisphere-joint-demo/hemisphere-joint-demo/cornelius_joint_mecha_demo.py b/python/hemisphere-joint-demo/hemisphere-joint-demo/cornelius_joint_mecha_demo.py
index 4187fb593..3b6b20c77 100755
--- a/python/hemisphere-joint-demo/hemisphere-joint-demo/cornelius_joint_mecha_demo.py
+++ b/python/hemisphere-joint-demo/hemisphere-joint-demo/cornelius_joint_mecha_demo.py
@@ -8,275 +8,6 @@ from armarx import remote_gui as rg
from armarx.remote_gui.widgets.ndarray import NdArrayWidget
-from numpy import pi
-
-
-# Function definitions
-def f_zenith(a1, a2, L, T_0):
- return -np.arcsin((2 * L ** 2 - 4 * L ** 2 * (L ** 5 * np.sin(T_0) * np.sin(T_0 + np.arcsin(a1 / L)) * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 3 - L ** 5 * np.sin(T_0) * np.sin(T_0 + np.arcsin(a1 / L)) * np.sin(
- T_0 + np.arcsin(a2 / L)) - L ** 5 * np.sin(T_0) * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 + L ** 5 * np.sin(
- T_0) - L * np.sqrt(2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 3 - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(
- T_0 + np.arcsin(a2 / L)) - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 3 + 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) * np.sin(
- T_0 + np.arcsin(a2 / L)) + 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 - 2 * L ** 8 * np.sin(T_0) ** 2 - L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 4 * np.sin(T_0 + np.arcsin(a2 / L)) ** 4 + L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 4 * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 + L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(T_0 + np.arcsin(a2 / L)) ** 4 - L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 - L ** 8 * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 + L ** 8) * np.sin(
- T_0 + np.arcsin(a2 / L))) ** 2 / ((-L ** 2 * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 + L ** 2) * (
- -L ** 4 * np.sin(T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + L ** 4) ** 2) - 4 * L ** 2 * (
- L ** 5 * np.sin(T_0) * np.sin(T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(
- T_0 + np.arcsin(a2 / L)) - L ** 5 * np.sin(T_0) * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 - L ** 5 * np.sin(T_0) * np.sin(
- T_0 + np.arcsin(a1 / L)) * np.sin(T_0 + np.arcsin(a2 / L)) + L ** 5 * np.sin(
- T_0) - L * np.sqrt(
- 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 3 - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(
- T_0 + np.arcsin(a2 / L)) - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 3 + 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) * np.sin(T_0 + np.arcsin(a2 / L)) + 2 * L ** 8 * np.sin(
- T_0) ** 2 * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 - 2 * L ** 8 * np.sin(
- T_0) ** 2 - L ** 8 * np.sin(T_0 + np.arcsin(a1 / L)) ** 4 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 4 + L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 4 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 4 - L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 - L ** 8 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + L ** 8) * np.sin(T_0 + np.arcsin(a1 / L))) ** 2 / (
- (-L ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) ** 2 + L ** 2) * (
- -L ** 4 * np.sin(T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + L ** 4) ** 2)) / (2 * L ** 2)) + pi / 2
-
-
-def f_azimuth(a1, a2, L, T_0):
- return np.arctan2(np.sqrt(4 * L ** 2 * np.sin(T_0) ** 2 - 4 * L ** 2 * (
- L ** 5 * np.sin(T_0) * np.sin(T_0 + np.arcsin(a1 / L)) * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 3 - L ** 5 * np.sin(T_0) * np.sin(T_0 + np.arcsin(a1 / L)) * np.sin(
- T_0 + np.arcsin(a2 / L)) - L ** 5 * np.sin(T_0) * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 + L ** 5 * np.sin(
- T_0) - L * np.sqrt(2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 3 - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(T_0 + np.arcsin(a2 / L)) - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 + 2 * L ** 8 * np.sin(
- T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) ** 2 - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) * np.sin(T_0 + np.arcsin(a2 / L)) ** 3 + 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) * np.sin(T_0 + np.arcsin(a2 / L)) + 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 - 2 * L ** 8 * np.sin(T_0) ** 2 - L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 4 * np.sin(T_0 + np.arcsin(a2 / L)) ** 4 + L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 4 * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 + L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(T_0 + np.arcsin(a2 / L)) ** 4 - L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 - L ** 8 * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 + L ** 8) * np.sin(
- T_0 + np.arcsin(a2 / L))) ** 2 * np.sin(T_0) ** 2 / (
- (-L ** 2 * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 + L ** 2) * (
- -L ** 4 * np.sin(T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + L ** 4) ** 2) - 4 * L ** 2 * (
- L ** 5 * np.sin(T_0) * np.sin(T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(
- T_0 + np.arcsin(a2 / L)) - L ** 5 * np.sin(T_0) * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 - L ** 5 * np.sin(T_0) * np.sin(
- T_0 + np.arcsin(a1 / L)) * np.sin(T_0 + np.arcsin(a2 / L)) + L ** 5 * np.sin(
- T_0) - L * np.sqrt(
- 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 3 - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(
- T_0 + np.arcsin(a2 / L)) - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 3 + 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) * np.sin(
- T_0 + np.arcsin(a2 / L)) + 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 - 2 * L ** 8 * np.sin(
- T_0) ** 2 - L ** 8 * np.sin(T_0 + np.arcsin(a1 / L)) ** 4 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 4 + L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 4 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 4 - L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 - L ** 8 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + L ** 8) * np.sin(
- T_0 + np.arcsin(a1 / L))) ** 2 * np.sin(T_0) ** 2 / (
- (-L ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) ** 2 + L ** 2) * (
- -L ** 4 * np.sin(T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + L ** 4) ** 2)) * (
- L ** 5 * np.sin(T_0) * np.sin(T_0 + np.arcsin(a1 / L)) * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 3 - L ** 5 * np.sin(T_0) * np.sin(
- T_0 + np.arcsin(a1 / L)) * np.sin(T_0 + np.arcsin(a2 / L)) - L ** 5 * np.sin(
- T_0) * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 + L ** 5 * np.sin(T_0) - L * np.sqrt(
- 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 3 - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(T_0 + np.arcsin(a2 / L)) - 2 * L ** 8 * np.sin(
- T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) * np.sin(T_0 + np.arcsin(a2 / L)) ** 3 + 2 * L ** 8 * np.sin(
- T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) * np.sin(
- T_0 + np.arcsin(a2 / L)) + 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 - 2 * L ** 8 * np.sin(T_0) ** 2 - L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 4 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 4 + L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 4 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 4 - L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 - L ** 8 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + L ** 8) * np.sin(T_0 + np.arcsin(a2 / L))) / (
- L * np.sqrt(-L ** 2 * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 + L ** 2) * (
- -L ** 4 * np.sin(T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + L ** 4) * np.sin(T_0)), np.sqrt(
- 4 * L ** 2 * np.sin(T_0) ** 2 - 4 * L ** 2 * (L ** 5 * np.sin(T_0) * np.sin(T_0 + np.arcsin(a1 / L)) * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 3 - L ** 5 * np.sin(T_0) * np.sin(T_0 + np.arcsin(a1 / L)) * np.sin(
- T_0 + np.arcsin(a2 / L)) - L ** 5 * np.sin(T_0) * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 + L ** 5 * np.sin(
- T_0) - L * np.sqrt(2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 3 - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(T_0 + np.arcsin(a2 / L)) - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 + 2 * L ** 8 * np.sin(
- T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) ** 2 - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) * np.sin(T_0 + np.arcsin(a2 / L)) ** 3 + 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) * np.sin(T_0 + np.arcsin(a2 / L)) + 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 - 2 * L ** 8 * np.sin(T_0) ** 2 - L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 4 * np.sin(T_0 + np.arcsin(a2 / L)) ** 4 + L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 4 * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 + L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(T_0 + np.arcsin(a2 / L)) ** 4 - L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 - L ** 8 * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 + L ** 8) * np.sin(
- T_0 + np.arcsin(a2 / L))) ** 2 * np.sin(T_0) ** 2 / (
- (-L ** 2 * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 + L ** 2) * (
- -L ** 4 * np.sin(T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + L ** 4) ** 2) - 4 * L ** 2 * (
- L ** 5 * np.sin(T_0) * np.sin(T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(
- T_0 + np.arcsin(a2 / L)) - L ** 5 * np.sin(T_0) * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 - L ** 5 * np.sin(T_0) * np.sin(T_0 + np.arcsin(a1 / L)) * np.sin(
- T_0 + np.arcsin(a2 / L)) + L ** 5 * np.sin(T_0) - L * np.sqrt(
- 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 3 - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(T_0 + np.arcsin(a2 / L)) - 2 * L ** 8 * np.sin(
- T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) * np.sin(T_0 + np.arcsin(a2 / L)) ** 3 + 2 * L ** 8 * np.sin(
- T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) * np.sin(T_0 + np.arcsin(a2 / L)) + 2 * L ** 8 * np.sin(
- T_0) ** 2 * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 - 2 * L ** 8 * np.sin(T_0) ** 2 - L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 4 * np.sin(T_0 + np.arcsin(a2 / L)) ** 4 + L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 4 * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 + L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(T_0 + np.arcsin(a2 / L)) ** 4 - L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 - L ** 8 * np.sin(T_0 + np.arcsin(a2 / L)) ** 2 + L ** 8) * np.sin(
- T_0 + np.arcsin(a1 / L))) ** 2 * np.sin(T_0) ** 2 / (
- (-L ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) ** 2 + L ** 2) * (
- -L ** 4 * np.sin(T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + L ** 4) ** 2)) * (
- L ** 5 * np.sin(T_0) * np.sin(T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(
- T_0 + np.arcsin(a2 / L)) - L ** 5 * np.sin(T_0) * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 - L ** 5 * np.sin(T_0) * np.sin(
- T_0 + np.arcsin(a1 / L)) * np.sin(T_0 + np.arcsin(a2 / L)) + L ** 5 * np.sin(
- T_0) - L * np.sqrt(
- 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 3 - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 3 * np.sin(T_0 + np.arcsin(a2 / L)) - 2 * L ** 8 * np.sin(
- T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 - 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a1 / L)) * np.sin(T_0 + np.arcsin(a2 / L)) ** 3 + 2 * L ** 8 * np.sin(
- T_0) ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) * np.sin(
- T_0 + np.arcsin(a2 / L)) + 2 * L ** 8 * np.sin(T_0) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 - 2 * L ** 8 * np.sin(T_0) ** 2 - L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 4 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 4 + L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 4 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 4 - L ** 8 * np.sin(
- T_0 + np.arcsin(a1 / L)) ** 2 - L ** 8 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + L ** 8) * np.sin(T_0 + np.arcsin(a1 / L))) / (
- L * np.sqrt(-L ** 2 * np.sin(T_0 + np.arcsin(a1 / L)) ** 2 + L ** 2) * (
- -L ** 4 * np.sin(T_0 + np.arcsin(a1 / L)) ** 2 * np.sin(
- T_0 + np.arcsin(a2 / L)) ** 2 + L ** 4) * np.sin(T_0)))
-
-
-def fk(P1_z, P2_z, L, T_0):
- """
- From:
- https://colab.research.google.com/drive/11faUc8pS1yWxFrnmt05VqpDsqOwEi_dg#scrollTo=za3fZw9Rq5d9&line=1&uniqifier=1
- """
- from numpy import sin, cos, sqrt
-
- PE_x = 2 * L * (L ** 5 * sin(T_0) - L ** 3 * P1_z * P2_z * sin(T_0) - L ** 3 * P2_z ** 2 * sin(
- T_0) + L * P1_z * P2_z ** 3 * sin(T_0) - P2_z * sqrt(
- -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * P1_z ** 2 * sin(
- T_0) ** 2 - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * sin(T_0) ** 2 + 2 * L ** 6 * P2_z ** 2 * sin(
- T_0) ** 2 - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * sin(
- T_0) ** 2 - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * sin(T_0) ** 2 - 2 * L ** 4 * P1_z * P2_z ** 3 * sin(
- T_0) ** 2 + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * sin(
- T_0) ** 2 + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * sin(T_0) / (
- sqrt(L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2))
- PE_y = 2 * L * (L ** 5 * sin(T_0) - L ** 3 * P1_z ** 2 * sin(T_0) - L ** 3 * P1_z * P2_z * sin(
- T_0) + L * P1_z ** 3 * P2_z * sin(T_0) - P1_z * sqrt(
- -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * P1_z ** 2 * sin(
- T_0) ** 2 - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * sin(T_0) ** 2 + 2 * L ** 6 * P2_z ** 2 * sin(
- T_0) ** 2 - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * sin(
- T_0) ** 2 - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * sin(T_0) ** 2 - 2 * L ** 4 * P1_z * P2_z ** 3 * sin(
- T_0) ** 2 + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * sin(
- T_0) ** 2 + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * sin(T_0) / (
- sqrt(L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2))
- PE_z = 2 * L * (L * (L ** 4 - P1_z ** 2 * P2_z ** 2) * (P1_z + P2_z) * sin(T_0) + (L ** 2 + P1_z * P2_z) * sqrt(
- L ** 2 * (L ** 2 * P1_z + L ** 2 * P2_z - P1_z ** 2 * P2_z - P1_z * P2_z ** 2) ** 2 * sin(T_0) ** 2 + (
- -L ** 4 + P1_z ** 2 * P2_z ** 2) * (-2 * L ** 4 * cos(T_0) ** 2 + L ** 4 + L ** 2 * P1_z ** 2 * cos(
- T_0) ** 2 + L ** 2 * P2_z ** 2 * cos(T_0) ** 2 - P1_z ** 2 * P2_z ** 2))) * sin(T_0) / (
- (L ** 2 + P1_z * P2_z) * (L ** 4 - P1_z ** 2 * P2_z ** 2))
-
- return np.array([PE_x, PE_y, PE_z])
-
-
-def fk_ori(PE_x, PE_y, L, T_0):
- """
- From:
- https://colab.research.google.com/drive/11faUc8pS1yWxFrnmt05VqpDsqOwEi_dg#scrollTo=aNSv-3Ftv3ps&line=2&uniqifier=1
- """
- from numpy import sin, cos, sqrt
-
- # @title Orientation vectors
- exx = 1 - PE_x ** 2 / (2 * L ** 2 * sin(T_0) ** 2)
- exy = -PE_x * PE_y / (2 * L ** 2 * sin(T_0) ** 2)
- exz = -PE_x * sqrt(4 * L ** 2 * sin(T_0) ** 2 - PE_x ** 2 - PE_y ** 2) / (2 * L ** 2 * sin(T_0) ** 2)
-
- eyx = -PE_x * PE_y / (2 * L ** 2 * sin(T_0) ** 2)
- eyy = 1 - PE_y ** 2 / (2 * L ** 2 * sin(T_0) ** 2)
- eyz = -PE_y * sqrt(4 * L ** 2 * sin(T_0) ** 2 - PE_x ** 2 - PE_y ** 2) / (2 * L ** 2 * sin(T_0) ** 2)
-
- ezx = PE_x * sqrt(4 * L ** 2 * sin(T_0) ** 2 - PE_x ** 2 - PE_y ** 2) / (2 * L ** 2 * sin(T_0) ** 2)
- ezy = PE_y * sqrt(4 * L ** 2 * sin(T_0) ** 2 - PE_x ** 2 - PE_y ** 2) / (2 * L ** 2 * sin(T_0) ** 2)
- ezz = (2 * L ** 2 - PE_x ** 2 / sin(T_0) ** 2 - PE_y ** 2 / sin(T_0) ** 2) / (2 * L ** 2)
-
-
- # @markdown for base change from Wrist to Base generate matrix from base vectors
- # https://de.wikipedia.org/wiki/Basiswechsel_(Vektorraum)
- r_wrist_to_base = np.array(
- [[exx, eyx, ezx],
- [exy, eyy, ezy],
- [exz, eyz, ezz]]
- )
- print(f"r_wrist_to_base:\n{np.round(r_wrist_to_base, 3)}")
-
- # @markdown since the mechanism is symmetric to the middle just z axis is inverted for Base to wrist
- r_base_to_wrist = np.array(
- [[ exx, eyx, ezx],
- [ exy, eyy, ezy],
- [-exz, -eyz, -ezz]]
- )
- print(f"r_base_to_wrist:\n{np.round(r_base_to_wrist, 3)}")
-
- return r_wrist_to_base
-
class Data:
@@ -296,20 +27,26 @@ class Data:
def fk(self):
print("-" * 50)
- # Dummy.
- if False:
- eef = np.zeros(3)
- eef += tf3d.axangles.axangle2mat((1, 0, 0), self.linear_joints[0]) @ self.links[0]
- eef += tf3d.axangles.axangle2mat((1, 0, 0), self.linear_joints[1]) @ self.links[1]
+ pos = self.eef_pos.copy()
+ ori = self.eef_ori.copy()
- self.eef_pos = eef
+ # Dummy.
+ do_revolute = False
+ if do_revolute:
+ pos = np.zeros(3)
+ pos += tf3d.axangles.axangle2mat((1, 0, 0), self.linear_joints[0]) @ self.links[0]
+ pos += tf3d.axangles.axangle2mat((1, 0, 0), self.linear_joints[1]) @ self.links[1]
# KIT-Wrist constants
lever = 1
theta_0 = np.deg2rad(25)
print(f"(a1, a2) = {self.linear_joints}")
- if False:
+ do_azim_zenith = False
+ if do_azim_zenith:
+ from armarx.math import spherical
+ from .equations import f_azimuth, f_zenith
+
azimuth = f_azimuth(*self.linear_joints, L=lever, T_0=theta_0)
zenith = f_zenith(*self.linear_joints, L=lever, T_0=theta_0)
radius = lever
@@ -318,12 +55,15 @@ class Data:
pos = spherical.spherical2cartesian([radius, azimuth, elevation])
print(f"(azimuth, zenith) = ({azimuth:.3f}, {zenith:.3f}) | Cartesian = {np.round(pos, 3)}")
- if True:
+ do_fk = True
+ if do_fk:
+ from .equations import fk, fk_ori
+
pos = fk(*self.linear_joints, L=lever, T_0=theta_0)
ori = fk_ori(pos[0], pos[1], L=lever, T_0=theta_0)
print(f"(x, y, z) = {np.round(pos, 3)}")
- self.eef_pos[:] = pos
+ self.eef_pos = pos
self.eef_ori = ori
def ik(self):
@@ -379,7 +119,9 @@ class WidgetsTab(rg.Tab):
self.visu = visu
self.arviz = arviz or viz.Client(tag)
- self.linear_joints = NdArrayWidget(data.linear_joints, row_vector=True, range_min=-1, range_max=1)
+ limit = 0.7
+ self.linear_joints = NdArrayWidget(data.linear_joints, row_vector=True,
+ range_min=-limit, range_max=limit)
self.eef_pos = NdArrayWidget(data.eef_pos, row_vector=True, range_min=-100, range_max=100)
def create_widget_tree(self) -> rg.GridLayout:
diff --git a/python/hemisphere-joint-demo/hemisphere-joint-demo/equations.py b/python/hemisphere-joint-demo/hemisphere-joint-demo/equations.py
new file mode 100755
index 000000000..ee003dce0
--- /dev/null
+++ b/python/hemisphere-joint-demo/hemisphere-joint-demo/equations.py
@@ -0,0 +1,1057 @@
+import numpy as np
+
+import transforms3d as tf3d
+import typing as ty
+
+from numpy import pi, sin, cos, sqrt
+from numpy import pi, sin, cos, sqrt, arcsin, arctan2
+
+from .terms import CommonTerms
+
+
+def f_zenith(a1, a2, L, T_0):
+ t = CommonTerms(L=L, T_0=T_0)
+
+ return -arcsin((2 * L ** 2 - 4 * L ** 2 * (L ** 5 * t.sin_t0 * sin(T_0 + arcsin(a1 / L)) * sin(
+ T_0 + arcsin(a2 / L)) ** 3 - L ** 5 * t.sin_t0 * sin(T_0 + arcsin(a1 / L)) * sin(
+ T_0 + arcsin(a2 / L)) - L ** 5 * t.sin_t0 * sin(T_0 + arcsin(a2 / L)) ** 2 + L ** 5 * t.sin_t0 - L * sqrt(2 * L ** 8 * t.sin_t0_sq * sin(T_0 + arcsin(a1 / L)) ** 3 * sin(
+ T_0 + arcsin(a2 / L)) ** 3 - 2 * L ** 8 * t.sin_t0_sq * sin(T_0 + arcsin(a1 / L)) ** 3 * sin(
+ T_0 + arcsin(a2 / L)) - 2 * L ** 8 * t.sin_t0_sq * sin(T_0 + arcsin(a1 / L)) ** 2 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) ** 2 - 2 * L ** 8 * t.sin_t0_sq * sin(T_0 + arcsin(a1 / L)) * sin(
+ T_0 + arcsin(a2 / L)) ** 3 + 2 * L ** 8 * t.sin_t0_sq * sin(T_0 + arcsin(a1 / L)) * sin(
+ T_0 + arcsin(a2 / L)) + 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a2 / L)) ** 2 - 2 * L ** 8 * t.sin_t0_sq - L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 4 * sin(T_0 + arcsin(a2 / L)) ** 4 + L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 4 * sin(T_0 + arcsin(a2 / L)) ** 2 + L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 * sin(T_0 + arcsin(a2 / L)) ** 4 - L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 - L ** 8 * sin(T_0 + arcsin(a2 / L)) ** 2 + L ** 8) * sin(
+ T_0 + arcsin(a2 / L))) ** 2 / ((-L ** 2 * sin(T_0 + arcsin(a2 / L)) ** 2 + L ** 2) * (
+ -L ** 4 * sin(T_0 + arcsin(a1 / L)) ** 2 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + L ** 4) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 * sin(T_0 + arcsin(a1 / L)) ** 3 * sin(
+ T_0 + arcsin(a2 / L)) - L ** 5 * t.sin_t0 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 - L ** 5 * t.sin_t0 * sin(
+ T_0 + arcsin(a1 / L)) * sin(T_0 + arcsin(a2 / L)) + L ** 5 * sin(
+ T_0) - L * sqrt(
+ 2 * L ** 8 * t.sin_t0_sq * sin(T_0 + arcsin(a1 / L)) ** 3 * sin(
+ T_0 + arcsin(a2 / L)) ** 3 - 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) ** 3 * sin(
+ T_0 + arcsin(a2 / L)) - 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) ** 2 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) ** 2 - 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) * sin(
+ T_0 + arcsin(a2 / L)) ** 3 + 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) * sin(T_0 + arcsin(a2 / L)) + 2 * L ** 8 * sin(
+ T_0) ** 2 * sin(T_0 + arcsin(a2 / L)) ** 2 - 2 * L ** 8 * sin(
+ T_0) ** 2 - L ** 8 * sin(T_0 + arcsin(a1 / L)) ** 4 * sin(
+ T_0 + arcsin(a2 / L)) ** 4 + L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 4 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 * sin(
+ T_0 + arcsin(a2 / L)) ** 4 - L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 - L ** 8 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + L ** 8) * sin(T_0 + arcsin(a1 / L))) ** 2 / (
+ (-L ** 2 * sin(T_0 + arcsin(a1 / L)) ** 2 + L ** 2) * (
+ -L ** 4 * sin(T_0 + arcsin(a1 / L)) ** 2 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + L ** 4) ** 2)) / (2 * L ** 2)) + pi / 2
+
+
+def f_azimuth(a1, a2, L, T_0):
+ t = CommonTerms(L=L, T_0=T_0)
+
+ return arctan2(sqrt(4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 * sin(T_0 + arcsin(a1 / L)) * sin(
+ T_0 + arcsin(a2 / L)) ** 3 - L ** 5 * t.sin_t0 * sin(T_0 + arcsin(a1 / L)) * sin(
+ T_0 + arcsin(a2 / L)) - L ** 5 * t.sin_t0 * sin(T_0 + arcsin(a2 / L)) ** 2 + L ** 5 * sin(
+ T_0) - L * sqrt(2 * L ** 8 * t.sin_t0_sq * sin(T_0 + arcsin(a1 / L)) ** 3 * sin(
+ T_0 + arcsin(a2 / L)) ** 3 - 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) ** 3 * sin(T_0 + arcsin(a2 / L)) - 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) ** 2 * sin(T_0 + arcsin(a2 / L)) ** 2 + 2 * L ** 8 * sin(
+ T_0) ** 2 * sin(T_0 + arcsin(a1 / L)) ** 2 - 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) * sin(T_0 + arcsin(a2 / L)) ** 3 + 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) * sin(T_0 + arcsin(a2 / L)) + 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a2 / L)) ** 2 - 2 * L ** 8 * t.sin_t0_sq - L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 4 * sin(T_0 + arcsin(a2 / L)) ** 4 + L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 4 * sin(T_0 + arcsin(a2 / L)) ** 2 + L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 * sin(T_0 + arcsin(a2 / L)) ** 4 - L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 - L ** 8 * sin(T_0 + arcsin(a2 / L)) ** 2 + L ** 8) * sin(
+ T_0 + arcsin(a2 / L))) ** 2 * t.sin_t0_sq / (
+ (-L ** 2 * sin(T_0 + arcsin(a2 / L)) ** 2 + L ** 2) * (
+ -L ** 4 * sin(T_0 + arcsin(a1 / L)) ** 2 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + L ** 4) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 * sin(T_0 + arcsin(a1 / L)) ** 3 * sin(
+ T_0 + arcsin(a2 / L)) - L ** 5 * t.sin_t0 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 - L ** 5 * t.sin_t0 * sin(
+ T_0 + arcsin(a1 / L)) * sin(T_0 + arcsin(a2 / L)) + L ** 5 * sin(
+ T_0) - L * sqrt(
+ 2 * L ** 8 * t.sin_t0_sq * sin(T_0 + arcsin(a1 / L)) ** 3 * sin(
+ T_0 + arcsin(a2 / L)) ** 3 - 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) ** 3 * sin(
+ T_0 + arcsin(a2 / L)) - 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) ** 2 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) ** 2 - 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) * sin(
+ T_0 + arcsin(a2 / L)) ** 3 + 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) * sin(
+ T_0 + arcsin(a2 / L)) + 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a2 / L)) ** 2 - 2 * L ** 8 * sin(
+ T_0) ** 2 - L ** 8 * sin(T_0 + arcsin(a1 / L)) ** 4 * sin(
+ T_0 + arcsin(a2 / L)) ** 4 + L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 4 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 * sin(
+ T_0 + arcsin(a2 / L)) ** 4 - L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 - L ** 8 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + L ** 8) * sin(
+ T_0 + arcsin(a1 / L))) ** 2 * t.sin_t0_sq / (
+ (-L ** 2 * sin(T_0 + arcsin(a1 / L)) ** 2 + L ** 2) * (
+ -L ** 4 * sin(T_0 + arcsin(a1 / L)) ** 2 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + L ** 4) ** 2)) * (
+ L ** 5 * t.sin_t0 * sin(T_0 + arcsin(a1 / L)) * sin(
+ T_0 + arcsin(a2 / L)) ** 3 - L ** 5 * t.sin_t0 * sin(
+ T_0 + arcsin(a1 / L)) * sin(T_0 + arcsin(a2 / L)) - L ** 5 * sin(
+ T_0) * sin(T_0 + arcsin(a2 / L)) ** 2 + L ** 5 * t.sin_t0 - L * sqrt(
+ 2 * L ** 8 * t.sin_t0_sq * sin(T_0 + arcsin(a1 / L)) ** 3 * sin(
+ T_0 + arcsin(a2 / L)) ** 3 - 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) ** 3 * sin(T_0 + arcsin(a2 / L)) - 2 * L ** 8 * sin(
+ T_0) ** 2 * sin(T_0 + arcsin(a1 / L)) ** 2 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) ** 2 - 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) * sin(T_0 + arcsin(a2 / L)) ** 3 + 2 * L ** 8 * sin(
+ T_0) ** 2 * sin(T_0 + arcsin(a1 / L)) * sin(
+ T_0 + arcsin(a2 / L)) + 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a2 / L)) ** 2 - 2 * L ** 8 * t.sin_t0_sq - L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 4 * sin(
+ T_0 + arcsin(a2 / L)) ** 4 + L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 4 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 * sin(
+ T_0 + arcsin(a2 / L)) ** 4 - L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 - L ** 8 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + L ** 8) * sin(T_0 + arcsin(a2 / L))) / (
+ L * sqrt(-L ** 2 * sin(T_0 + arcsin(a2 / L)) ** 2 + L ** 2) * (
+ -L ** 4 * sin(T_0 + arcsin(a1 / L)) ** 2 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + L ** 4) * t.sin_t0), sqrt(
+ 4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (L ** 5 * t.sin_t0 * sin(T_0 + arcsin(a1 / L)) * sin(
+ T_0 + arcsin(a2 / L)) ** 3 - L ** 5 * t.sin_t0 * sin(T_0 + arcsin(a1 / L)) * sin(
+ T_0 + arcsin(a2 / L)) - L ** 5 * t.sin_t0 * sin(T_0 + arcsin(a2 / L)) ** 2 + L ** 5 * sin(
+ T_0) - L * sqrt(2 * L ** 8 * t.sin_t0_sq * sin(T_0 + arcsin(a1 / L)) ** 3 * sin(
+ T_0 + arcsin(a2 / L)) ** 3 - 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) ** 3 * sin(T_0 + arcsin(a2 / L)) - 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) ** 2 * sin(T_0 + arcsin(a2 / L)) ** 2 + 2 * L ** 8 * sin(
+ T_0) ** 2 * sin(T_0 + arcsin(a1 / L)) ** 2 - 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) * sin(T_0 + arcsin(a2 / L)) ** 3 + 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) * sin(T_0 + arcsin(a2 / L)) + 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a2 / L)) ** 2 - 2 * L ** 8 * t.sin_t0_sq - L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 4 * sin(T_0 + arcsin(a2 / L)) ** 4 + L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 4 * sin(T_0 + arcsin(a2 / L)) ** 2 + L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 * sin(T_0 + arcsin(a2 / L)) ** 4 - L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 - L ** 8 * sin(T_0 + arcsin(a2 / L)) ** 2 + L ** 8) * sin(
+ T_0 + arcsin(a2 / L))) ** 2 * t.sin_t0_sq / (
+ (-L ** 2 * sin(T_0 + arcsin(a2 / L)) ** 2 + L ** 2) * (
+ -L ** 4 * sin(T_0 + arcsin(a1 / L)) ** 2 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + L ** 4) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 * sin(T_0 + arcsin(a1 / L)) ** 3 * sin(
+ T_0 + arcsin(a2 / L)) - L ** 5 * t.sin_t0 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 - L ** 5 * t.sin_t0 * sin(T_0 + arcsin(a1 / L)) * sin(
+ T_0 + arcsin(a2 / L)) + L ** 5 * t.sin_t0 - L * sqrt(
+ 2 * L ** 8 * t.sin_t0_sq * sin(T_0 + arcsin(a1 / L)) ** 3 * sin(
+ T_0 + arcsin(a2 / L)) ** 3 - 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) ** 3 * sin(T_0 + arcsin(a2 / L)) - 2 * L ** 8 * sin(
+ T_0) ** 2 * sin(T_0 + arcsin(a1 / L)) ** 2 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) ** 2 - 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) * sin(T_0 + arcsin(a2 / L)) ** 3 + 2 * L ** 8 * sin(
+ T_0) ** 2 * sin(T_0 + arcsin(a1 / L)) * sin(T_0 + arcsin(a2 / L)) + 2 * L ** 8 * sin(
+ T_0) ** 2 * sin(T_0 + arcsin(a2 / L)) ** 2 - 2 * L ** 8 * t.sin_t0_sq - L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 4 * sin(T_0 + arcsin(a2 / L)) ** 4 + L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 4 * sin(T_0 + arcsin(a2 / L)) ** 2 + L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 * sin(T_0 + arcsin(a2 / L)) ** 4 - L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 - L ** 8 * sin(T_0 + arcsin(a2 / L)) ** 2 + L ** 8) * sin(
+ T_0 + arcsin(a1 / L))) ** 2 * t.sin_t0_sq / (
+ (-L ** 2 * sin(T_0 + arcsin(a1 / L)) ** 2 + L ** 2) * (
+ -L ** 4 * sin(T_0 + arcsin(a1 / L)) ** 2 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + L ** 4) ** 2)) * (
+ L ** 5 * t.sin_t0 * sin(T_0 + arcsin(a1 / L)) ** 3 * sin(
+ T_0 + arcsin(a2 / L)) - L ** 5 * t.sin_t0 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 - L ** 5 * t.sin_t0 * sin(
+ T_0 + arcsin(a1 / L)) * sin(T_0 + arcsin(a2 / L)) + L ** 5 * sin(
+ T_0) - L * sqrt(
+ 2 * L ** 8 * t.sin_t0_sq * sin(T_0 + arcsin(a1 / L)) ** 3 * sin(
+ T_0 + arcsin(a2 / L)) ** 3 - 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) ** 3 * sin(T_0 + arcsin(a2 / L)) - 2 * L ** 8 * sin(
+ T_0) ** 2 * sin(T_0 + arcsin(a1 / L)) ** 2 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) ** 2 - 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a1 / L)) * sin(T_0 + arcsin(a2 / L)) ** 3 + 2 * L ** 8 * sin(
+ T_0) ** 2 * sin(T_0 + arcsin(a1 / L)) * sin(
+ T_0 + arcsin(a2 / L)) + 2 * L ** 8 * t.sin_t0_sq * sin(
+ T_0 + arcsin(a2 / L)) ** 2 - 2 * L ** 8 * t.sin_t0_sq - L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 4 * sin(
+ T_0 + arcsin(a2 / L)) ** 4 + L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 4 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 * sin(
+ T_0 + arcsin(a2 / L)) ** 4 - L ** 8 * sin(
+ T_0 + arcsin(a1 / L)) ** 2 - L ** 8 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + L ** 8) * sin(T_0 + arcsin(a1 / L))) / (
+ L * sqrt(-L ** 2 * sin(T_0 + arcsin(a1 / L)) ** 2 + L ** 2) * (
+ -L ** 4 * sin(T_0 + arcsin(a1 / L)) ** 2 * sin(
+ T_0 + arcsin(a2 / L)) ** 2 + L ** 4) * t.sin_t0))
+
+
+def fk(P1_z, P2_z, L, T_0):
+ """
+ From:
+ https://colab.research.google.com/drive/11faUc8pS1yWxFrnmt05VqpDsqOwEi_dg#scrollTo=za3fZw9Rq5d9&line=1&uniqifier=1
+ """
+ t = CommonTerms(L=L, T_0=T_0)
+
+ PE_x = 2 * L * (L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * sin(
+ T_0) ** 2 - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * sin(
+ T_0) ** 2 - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * sin(
+ T_0) ** 2 - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * sin(
+ T_0) ** 2 + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * sin(
+ T_0) ** 2 + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0 / (
+ sqrt(L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2))
+ PE_y = 2 * L * (L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * sin(
+ T_0) ** 2 - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * sin(
+ T_0) ** 2 - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * sin(
+ T_0) ** 2 - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * sin(
+ T_0) ** 2 + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * sin(
+ T_0) ** 2 + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0 / (
+ sqrt(L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2))
+ PE_z = 2 * L * (L * (L ** 4 - P1_z ** 2 * P2_z ** 2) * (P1_z + P2_z) * t.sin_t0 + (L ** 2 + P1_z * P2_z) * sqrt(
+ L ** 2 * (L ** 2 * P1_z + L ** 2 * P2_z - P1_z ** 2 * P2_z - P1_z * P2_z ** 2) ** 2 * t.sin_t0_sq + (
+ -L ** 4 + P1_z ** 2 * P2_z ** 2) * (-2 * L ** 4 * t.cos_t0_sq + L ** 4 + L ** 2 * P1_z ** 2 * cos(
+ T_0) ** 2 + L ** 2 * P2_z ** 2 * t.cos_t0_sq - P1_z ** 2 * P2_z ** 2))) * t.sin_t0 / (
+ (L ** 2 + P1_z * P2_z) * (L ** 4 - P1_z ** 2 * P2_z ** 2))
+
+ return np.array([PE_x, PE_y, PE_z])
+
+
+def fk_ori(PE_x, PE_y, L, T_0):
+ """
+ From:
+ https://colab.research.google.com/drive/11faUc8pS1yWxFrnmt05VqpDsqOwEi_dg#scrollTo=aNSv-3Ftv3ps&line=2&uniqifier=1
+ """
+ t = CommonTerms(L=L, T_0=T_0)
+
+ # @title Orientation vectors
+ exx = 1 - PE_x ** 2 / (2 * L ** 2 * t.sin_t0_sq)
+ exy = -PE_x * PE_y / (2 * L ** 2 * t.sin_t0_sq)
+ exz = -PE_x * sqrt(4 * L ** 2 * t.sin_t0_sq - PE_x ** 2 - PE_y ** 2) / (2 * L ** 2 * t.sin_t0_sq)
+
+ eyx = -PE_x * PE_y / (2 * L ** 2 * t.sin_t0_sq)
+ eyy = 1 - PE_y ** 2 / (2 * L ** 2 * t.sin_t0_sq)
+ eyz = -PE_y * sqrt(4 * L ** 2 * t.sin_t0_sq - PE_x ** 2 - PE_y ** 2) / (2 * L ** 2 * t.sin_t0_sq)
+
+ ezx = PE_x * sqrt(4 * L ** 2 * t.sin_t0_sq - PE_x ** 2 - PE_y ** 2) / (2 * L ** 2 * t.sin_t0_sq)
+ ezy = PE_y * sqrt(4 * L ** 2 * t.sin_t0_sq - PE_x ** 2 - PE_y ** 2) / (2 * L ** 2 * t.sin_t0_sq)
+ ezz = (2 * L ** 2 - PE_x ** 2 / t.sin_t0_sq - PE_y ** 2 / t.sin_t0_sq) / (2 * L ** 2)
+
+
+ # @markdown for base change from Wrist to Base generate matrix from base vectors
+ # https://de.wikipedia.org/wiki/Basiswechsel_(Vektorraum)
+ r_wrist_to_base = np.array(
+ [[exx, eyx, ezx],
+ [exy, eyy, ezy],
+ [exz, eyz, ezz]]
+ )
+ print(f"r_wrist_to_base:\n{np.round(r_wrist_to_base, 3)}")
+
+ # @markdown since the mechanism is symmetric to the middle just z axis is inverted for Base to wrist
+ r_base_to_wrist = np.array(
+ [[ exx, eyx, ezx],
+ [ exy, eyy, ezy],
+ [-exz, -eyz, -ezz]]
+ )
+ print(f"r_base_to_wrist:\n{np.round(r_base_to_wrist, 3)}")
+
+ return r_wrist_to_base
+
+
+
+def jacobian(P1_z, P2_z, L, T_0):
+ t = CommonTerms(L=L, T_0=T_0)
+
+ jac = np.zeros(6)
+ jac[0] = [4 * L * P1_z * P2_z ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0 / (
+ sqrt(L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) + 2 * L * (
+ -L ** 3 * P2_z * t.sin_t0 + L * P2_z ** 3 * t.sin_t0 - P2_z * (
+ 2 * L ** 6 * P1_z * t.sin_t0_sq - L ** 6 * P1_z + L ** 6 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq - L ** 4 * P2_z ** 3 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 3 * P2_z ** 2 + 3 * L ** 2 * P1_z ** 2 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z * P2_z ** 4 - 2 * P1_z ** 3 * P2_z ** 4) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0 / (
+ sqrt(L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2)), 4 * L * P1_z ** 2 * P2_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0 / (
+ sqrt(L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) + 2 * L * P2_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0 / (
+ (L ** 2 - P2_z ** 2) ** (3 / 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2)) + 2 * L * (
+ -L ** 3 * P1_z * t.sin_t0 - 2 * L ** 3 * P2_z * t.sin_t0 + 3 * L * P1_z * P2_z ** 2 * t.sin_t0 - P2_z * (
+ L ** 6 * P1_z * t.sin_t0_sq + 2 * L ** 6 * P2_z * t.sin_t0_sq - L ** 6 * P2_z - L ** 4 * P1_z ** 3 * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z + 3 * L ** 2 * P1_z ** 3 * P2_z ** 2 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 2 * P2_z ** 3 - 2 * P1_z ** 4 * P2_z ** 3) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4) - sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0 / (
+ sqrt(L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2))]
+ jac[1] = [4 * L * P1_z * P2_z ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0 / (
+ sqrt(L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) + 2 * L * P1_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0 / (
+ (L ** 2 - P1_z ** 2) ** (3 / 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2)) + 2 * L * (
+ -2 * L ** 3 * P1_z * t.sin_t0 - L ** 3 * P2_z * t.sin_t0 + 3 * L * P1_z ** 2 * P2_z * t.sin_t0 - P1_z * (
+ 2 * L ** 6 * P1_z * t.sin_t0_sq - L ** 6 * P1_z + L ** 6 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq - L ** 4 * P2_z ** 3 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 3 * P2_z ** 2 + 3 * L ** 2 * P1_z ** 2 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z * P2_z ** 4 - 2 * P1_z ** 3 * P2_z ** 4) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4) - sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0 / (
+ sqrt(L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2)), 4 * L * P1_z ** 2 * P2_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0 / (
+ sqrt(L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) + 2 * L * (
+ -L ** 3 * P1_z * t.sin_t0 + L * P1_z ** 3 * t.sin_t0 - P1_z * (
+ L ** 6 * P1_z * t.sin_t0_sq + 2 * L ** 6 * P2_z * t.sin_t0_sq - L ** 6 * P2_z - L ** 4 * P1_z ** 3 * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z + 3 * L ** 2 * P1_z ** 3 * P2_z ** 2 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 2 * P2_z ** 3 - 2 * P1_z ** 4 * P2_z ** 3) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0 / (
+ sqrt(L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2))]
+ jac[2] = [4 * L * P1_z * P2_z ** 2 * (
+ L * (L ** 4 - P1_z ** 2 * P2_z ** 2) * (P1_z + P2_z) * t.sin_t0 + (L ** 2 + P1_z * P2_z) * sqrt(
+ L ** 2 * (L ** 2 * P1_z + L ** 2 * P2_z - P1_z ** 2 * P2_z - P1_z * P2_z ** 2) ** 2 * t.sin_t0_sq + (
+ -L ** 4 + P1_z ** 2 * P2_z ** 2) * (
+ -2 * L ** 4 * t.cos_t0_sq + L ** 4 + L ** 2 * P1_z ** 2 * t.cos_t0_sq + L ** 2 * P2_z ** 2 * t.cos_t0_sq - P1_z ** 2 * P2_z ** 2))) * t.sin_t0 / (
+ (L ** 2 + P1_z * P2_z) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 2 * L * P2_z * (
+ L * (L ** 4 - P1_z ** 2 * P2_z ** 2) * (P1_z + P2_z) * t.sin_t0 + (L ** 2 + P1_z * P2_z) * sqrt(
+ L ** 2 * (L ** 2 * P1_z + L ** 2 * P2_z - P1_z ** 2 * P2_z - P1_z * P2_z ** 2) ** 2 * t.sin_t0_sq + (
+ -L ** 4 + P1_z ** 2 * P2_z ** 2) * (
+ -2 * L ** 4 * t.cos_t0_sq + L ** 4 + L ** 2 * P1_z ** 2 * t.cos_t0_sq + L ** 2 * P2_z ** 2 * t.cos_t0_sq - P1_z ** 2 * P2_z ** 2))) * t.sin_t0 / (
+ (L ** 2 + P1_z * P2_z) ** 2 * (L ** 4 - P1_z ** 2 * P2_z ** 2)) + 2 * L * (
+ -2 * L * P1_z * P2_z ** 2 * (P1_z + P2_z) * t.sin_t0 + L * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) * t.sin_t0 + P2_z * sqrt(
+ L ** 2 * (L ** 2 * P1_z + L ** 2 * P2_z - P1_z ** 2 * P2_z - P1_z * P2_z ** 2) ** 2 * t.sin_t0_sq + (
+ -L ** 4 + P1_z ** 2 * P2_z ** 2) * (
+ -2 * L ** 4 * t.cos_t0_sq + L ** 4 + L ** 2 * P1_z ** 2 * t.cos_t0_sq + L ** 2 * P2_z ** 2 * t.cos_t0_sq - P1_z ** 2 * P2_z ** 2)) + (
+ L ** 2 + P1_z * P2_z) * (L ** 2 * (2 * L ** 2 - 4 * P1_z * P2_z - 2 * P2_z ** 2) * (
+ L ** 2 * P1_z + L ** 2 * P2_z - P1_z ** 2 * P2_z - P1_z * P2_z ** 2) * t.sin_t0_sq / 2 + P1_z * P2_z ** 2 * (
+ -2 * L ** 4 * t.cos_t0_sq + L ** 4 + L ** 2 * P1_z ** 2 * t.cos_t0_sq + L ** 2 * P2_z ** 2 * t.cos_t0_sq - P1_z ** 2 * P2_z ** 2) + (
+ -L ** 4 + P1_z ** 2 * P2_z ** 2) * (
+ 2 * L ** 2 * P1_z * t.cos_t0_sq - 2 * P1_z * P2_z ** 2) / 2) / sqrt(
+ L ** 2 * (L ** 2 * P1_z + L ** 2 * P2_z - P1_z ** 2 * P2_z - P1_z * P2_z ** 2) ** 2 * t.sin_t0_sq + (
+ -L ** 4 + P1_z ** 2 * P2_z ** 2) * (
+ -2 * L ** 4 * t.cos_t0_sq + L ** 4 + L ** 2 * P1_z ** 2 * t.cos_t0_sq + L ** 2 * P2_z ** 2 * t.cos_t0_sq - P1_z ** 2 * P2_z ** 2))) * t.sin_t0 / (
+ (L ** 2 + P1_z * P2_z) * (L ** 4 - P1_z ** 2 * P2_z ** 2)), 4 * L * P1_z ** 2 * P2_z * (
+ L * (L ** 4 - P1_z ** 2 * P2_z ** 2) * (P1_z + P2_z) * t.sin_t0 + (L ** 2 + P1_z * P2_z) * sqrt(
+ L ** 2 * (L ** 2 * P1_z + L ** 2 * P2_z - P1_z ** 2 * P2_z - P1_z * P2_z ** 2) ** 2 * t.sin_t0_sq + (
+ -L ** 4 + P1_z ** 2 * P2_z ** 2) * (
+ -2 * L ** 4 * t.cos_t0_sq + L ** 4 + L ** 2 * P1_z ** 2 * t.cos_t0_sq + L ** 2 * P2_z ** 2 * t.cos_t0_sq - P1_z ** 2 * P2_z ** 2))) * t.sin_t0 / (
+ (L ** 2 + P1_z * P2_z) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 2 * L * P1_z * (
+ L * (L ** 4 - P1_z ** 2 * P2_z ** 2) * (P1_z + P2_z) * t.sin_t0 + (L ** 2 + P1_z * P2_z) * sqrt(
+ L ** 2 * (L ** 2 * P1_z + L ** 2 * P2_z - P1_z ** 2 * P2_z - P1_z * P2_z ** 2) ** 2 * t.sin_t0_sq + (
+ -L ** 4 + P1_z ** 2 * P2_z ** 2) * (
+ -2 * L ** 4 * t.cos_t0_sq + L ** 4 + L ** 2 * P1_z ** 2 * t.cos_t0_sq + L ** 2 * P2_z ** 2 * t.cos_t0_sq - P1_z ** 2 * P2_z ** 2))) * t.sin_t0 / (
+ (L ** 2 + P1_z * P2_z) ** 2 * (L ** 4 - P1_z ** 2 * P2_z ** 2)) + 2 * L * (
+ -2 * L * P1_z ** 2 * P2_z * (P1_z + P2_z) * t.sin_t0 + L * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) * t.sin_t0 + P1_z * sqrt(
+ L ** 2 * (L ** 2 * P1_z + L ** 2 * P2_z - P1_z ** 2 * P2_z - P1_z * P2_z ** 2) ** 2 * t.sin_t0_sq + (
+ -L ** 4 + P1_z ** 2 * P2_z ** 2) * (
+ -2 * L ** 4 * t.cos_t0_sq + L ** 4 + L ** 2 * P1_z ** 2 * t.cos_t0_sq + L ** 2 * P2_z ** 2 * t.cos_t0_sq - P1_z ** 2 * P2_z ** 2)) + (
+ L ** 2 + P1_z * P2_z) * (L ** 2 * (2 * L ** 2 - 2 * P1_z ** 2 - 4 * P1_z * P2_z) * (
+ L ** 2 * P1_z + L ** 2 * P2_z - P1_z ** 2 * P2_z - P1_z * P2_z ** 2) * t.sin_t0_sq / 2 + P1_z ** 2 * P2_z * (
+ -2 * L ** 4 * t.cos_t0_sq + L ** 4 + L ** 2 * P1_z ** 2 * t.cos_t0_sq + L ** 2 * P2_z ** 2 * t.cos_t0_sq - P1_z ** 2 * P2_z ** 2) + (
+ -L ** 4 + P1_z ** 2 * P2_z ** 2) * (
+ 2 * L ** 2 * P2_z * t.cos_t0_sq - 2 * P1_z ** 2 * P2_z) / 2) / sqrt(
+ L ** 2 * (L ** 2 * P1_z + L ** 2 * P2_z - P1_z ** 2 * P2_z - P1_z * P2_z ** 2) ** 2 * t.sin_t0_sq + (
+ -L ** 4 + P1_z ** 2 * P2_z ** 2) * (
+ -2 * L ** 4 * t.cos_t0_sq + L ** 4 + L ** 2 * P1_z ** 2 * t.cos_t0_sq + L ** 2 * P2_z ** 2 * t.cos_t0_sq - P1_z ** 2 * P2_z ** 2))) * t.sin_t0 / (
+ (L ** 2 + P1_z * P2_z) * (L ** 4 - P1_z ** 2 * P2_z ** 2))]
+ jac[3] = [
+
+ -(2 * L ** 2 - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (2 * P1_z * P2_z ** 2 * sqrt(
+ 4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * sin(
+ T_0) - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * sin(
+ T_0) - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * sin(
+ T_0) - L ** 3 * P1_z * P2_z * sin(
+ T_0) + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ L * sqrt(
+ L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2 * t.sin_t0) + P1_z * sqrt(
+ 4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * sin(
+ T_0) - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * sin(
+ T_0) - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * sin(
+ T_0) - L ** 3 * P1_z * P2_z * sin(
+ T_0) + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ L * (
+ L ** 2 - P1_z ** 2) ** (
+ 3 / 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) * t.sin_t0) + sqrt(
+ 4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * sin(
+ T_0) - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * sin(
+ T_0) - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ -2 * L ** 3 * P1_z * t.sin_t0 - L ** 3 * P2_z * sin(
+ T_0) + 3 * L * P1_z ** 2 * P2_z * sin(
+ T_0) - P1_z * (
+ 2 * L ** 6 * P1_z * t.sin_t0_sq - L ** 6 * P1_z + L ** 6 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq - L ** 4 * P2_z ** 3 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 3 * P2_z ** 2 + 3 * L ** 2 * P1_z ** 2 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z * P2_z ** 4 - 2 * P1_z ** 3 * P2_z ** 4) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4) - sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ L * sqrt(
+ L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) * t.sin_t0) + (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * sin(
+ T_0) - L ** 3 * P1_z * P2_z * sin(
+ T_0) + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ -8 * L ** 2 * P1_z * P2_z ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 3) - 8 * L ** 2 * P1_z * P2_z ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (
+ L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 3) - 4 * L ** 2 * P1_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (
+ L ** 2 - P1_z ** 2) ** 2 * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 2 * L ** 2 * (
+ -2 * L ** 3 * P2_z * t.sin_t0 + 2 * L * P2_z ** 3 * t.sin_t0 - 2 * P2_z * (
+ 2 * L ** 6 * P1_z * t.sin_t0_sq - L ** 6 * P1_z + L ** 6 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq - L ** 4 * P2_z ** 3 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 3 * P2_z ** 2 + 3 * L ** 2 * P1_z ** 2 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z * P2_z ** 4 - 2 * P1_z ** 3 * P2_z ** 4) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0_sq / (
+ (
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 2 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ -4 * L ** 3 * P1_z * t.sin_t0 - 2 * L ** 3 * P2_z * t.sin_t0 + 6 * L * P1_z ** 2 * P2_z * t.sin_t0 - 2 * P1_z * (
+ 2 * L ** 6 * P1_z * t.sin_t0_sq - L ** 6 * P1_z + L ** 6 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq - L ** 4 * P2_z ** 3 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 3 * P2_z ** 2 + 3 * L ** 2 * P1_z ** 2 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z * P2_z ** 4 - 2 * P1_z ** 3 * P2_z ** 4) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4) - 2 * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0_sq / (
+ (
+ L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) / (
+ L * sqrt(
+ L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) * sqrt(
+ 4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * t.sin_t0)) / (
+ 2 * L ** 2 * ((4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ L ** 2 * (L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2 * t.sin_t0_sq) + (
+ 2 * L ** 2 - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) ** 2 / (4 * L ** 4))) + sqrt(
+ 4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ -16 * L ** 2 * P1_z * P2_z ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 3) - 16 * L ** 2 * P1_z * P2_z ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 3) - 8 * L ** 2 * P1_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P1_z ** 2) ** 2 * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ -2 * L ** 3 * P2_z * t.sin_t0 + 2 * L * P2_z ** 3 * t.sin_t0 - 2 * P2_z * (
+ 2 * L ** 6 * P1_z * t.sin_t0_sq - L ** 6 * P1_z + L ** 6 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq - L ** 4 * P2_z ** 3 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 3 * P2_z ** 2 + 3 * L ** 2 * P1_z ** 2 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z * P2_z ** 4 - 2 * P1_z ** 3 * P2_z ** 4) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ -4 * L ** 3 * P1_z * t.sin_t0 - 2 * L ** 3 * P2_z * t.sin_t0 + 6 * L * P1_z ** 2 * P2_z * t.sin_t0 - 2 * P1_z * (
+ 2 * L ** 6 * P1_z * t.sin_t0_sq - L ** 6 * P1_z + L ** 6 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq - L ** 4 * P2_z ** 3 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 3 * P2_z ** 2 + 3 * L ** 2 * P1_z ** 2 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z * P2_z ** 4 - 2 * P1_z ** 3 * P2_z ** 4) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4) - 2 * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) / (
+ 2 * L ** 3 * sqrt(L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) * ((
+ 4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (
+ L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ L ** 2 * (
+ L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2 * t.sin_t0_sq) + (
+ 2 * L ** 2 - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (
+ L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) ** 2 / (
+ 4 * L ** 4)) * t.sin_t0),
+ -(2 * L ** 2 - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (2 * P1_z ** 2 * P2_z * sqrt(
+ 4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ L * sqrt(
+ L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2 * t.sin_t0) + sqrt(
+ 4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ -L ** 3 * P1_z * t.sin_t0 + L * P1_z ** 3 * t.sin_t0 - P1_z * (
+ L ** 6 * P1_z * t.sin_t0_sq + 2 * L ** 6 * P2_z * t.sin_t0_sq - L ** 6 * P2_z - L ** 4 * P1_z ** 3 * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z + 3 * L ** 2 * P1_z ** 3 * P2_z ** 2 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 2 * P2_z ** 3 - 2 * P1_z ** 4 * P2_z ** 3) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ L * sqrt(
+ L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) * t.sin_t0) + (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ -8 * L ** 2 * P1_z ** 2 * P2_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 3) - 8 * L ** 2 * P1_z ** 2 * P2_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (
+ L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 3) - 4 * L ** 2 * P2_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (
+ L ** 2 - P2_z ** 2) ** 2 * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 2 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ -2 * L ** 3 * P1_z * t.sin_t0 - 4 * L ** 3 * P2_z * t.sin_t0 + 6 * L * P1_z * P2_z ** 2 * t.sin_t0 - 2 * P2_z * (
+ L ** 6 * P1_z * t.sin_t0_sq + 2 * L ** 6 * P2_z * t.sin_t0_sq - L ** 6 * P2_z - L ** 4 * P1_z ** 3 * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z + 3 * L ** 2 * P1_z ** 3 * P2_z ** 2 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 2 * P2_z ** 3 - 2 * P1_z ** 4 * P2_z ** 3) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4) - 2 * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0_sq / (
+ (
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 2 * L ** 2 * (
+ -2 * L ** 3 * P1_z * t.sin_t0 + 2 * L * P1_z ** 3 * t.sin_t0 - 2 * P1_z * (
+ L ** 6 * P1_z * t.sin_t0_sq + 2 * L ** 6 * P2_z * t.sin_t0_sq - L ** 6 * P2_z - L ** 4 * P1_z ** 3 * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z + 3 * L ** 2 * P1_z ** 3 * P2_z ** 2 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 2 * P2_z ** 3 - 2 * P1_z ** 4 * P2_z ** 3) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0_sq / (
+ (
+ L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) / (
+ L * sqrt(
+ L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) * sqrt(
+ 4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * t.sin_t0)) / (
+ 2 * L ** 2 * ((4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * sin(
+ T_0) - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * sin(
+ T_0) - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * sin(
+ T_0) - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * sin(
+ T_0) - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * sin(
+ T_0) - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ L ** 2 * (L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2 * t.sin_t0_sq) + (2 * L ** 2 - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * sin(
+ T_0) - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (
+ L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) ** 2 / (
+ 4 * L ** 4))) + sqrt(4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ -16 * L ** 2 * P1_z ** 2 * P2_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 3) - 16 * L ** 2 * P1_z ** 2 * P2_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 3) - 8 * L ** 2 * P2_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P2_z ** 2) ** 2 * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ -2 * L ** 3 * P1_z * t.sin_t0 - 4 * L ** 3 * P2_z * t.sin_t0 + 6 * L * P1_z * P2_z ** 2 * t.sin_t0 - 2 * P2_z * (
+ L ** 6 * P1_z * t.sin_t0_sq + 2 * L ** 6 * P2_z * t.sin_t0_sq - L ** 6 * P2_z - L ** 4 * P1_z ** 3 * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z + 3 * L ** 2 * P1_z ** 3 * P2_z ** 2 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 2 * P2_z ** 3 - 2 * P1_z ** 4 * P2_z ** 3) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4) - 2 * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ -2 * L ** 3 * P1_z * t.sin_t0 + 2 * L * P1_z ** 3 * t.sin_t0 - 2 * P1_z * (
+ L ** 6 * P1_z * t.sin_t0_sq + 2 * L ** 6 * P2_z * t.sin_t0_sq - L ** 6 * P2_z - L ** 4 * P1_z ** 3 * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z + 3 * L ** 2 * P1_z ** 3 * P2_z ** 2 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 2 * P2_z ** 3 - 2 * P1_z ** 4 * P2_z ** 3) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) / (
+ 2 * L ** 3 * sqrt(L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) * ((
+ 4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (
+ L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ L ** 2 * (
+ L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2 * t.sin_t0_sq) + (
+ 2 * L ** 2 - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (
+ L ** 2 - P1_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) ** 2 / (
+ 4 * L ** 4)) * t.sin_t0)
+
+ ]
+ jac[4] = [-(2 * P1_z * P2_z ** 2 * sqrt(4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ L * sqrt(L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2 * t.sin_t0) + sqrt(
+ 4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ -L ** 3 * P2_z * t.sin_t0 + L * P2_z ** 3 * t.sin_t0 - P2_z * (
+ 2 * L ** 6 * P1_z * t.sin_t0_sq - L ** 6 * P1_z + L ** 6 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq - L ** 4 * P2_z ** 3 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 3 * P2_z ** 2 + 3 * L ** 2 * P1_z ** 2 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z * P2_z ** 4 - 2 * P1_z ** 3 * P2_z ** 4) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ L * sqrt(L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) * t.sin_t0) + (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ -8 * L ** 2 * P1_z * P2_z ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 3) - 8 * L ** 2 * P1_z * P2_z ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 3) - 4 * L ** 2 * P1_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) ** 2 * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 2 * L ** 2 * (
+ -2 * L ** 3 * P2_z * t.sin_t0 + 2 * L * P2_z ** 3 * t.sin_t0 - 2 * P2_z * (
+ 2 * L ** 6 * P1_z * t.sin_t0_sq - L ** 6 * P1_z + L ** 6 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq - L ** 4 * P2_z ** 3 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 3 * P2_z ** 2 + 3 * L ** 2 * P1_z ** 2 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z * P2_z ** 4 - 2 * P1_z ** 3 * P2_z ** 4) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 2 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ -4 * L ** 3 * P1_z * t.sin_t0 - 2 * L ** 3 * P2_z * t.sin_t0 + 6 * L * P1_z ** 2 * P2_z * t.sin_t0 - 2 * P1_z * (
+ 2 * L ** 6 * P1_z * t.sin_t0_sq - L ** 6 * P1_z + L ** 6 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq - L ** 4 * P2_z ** 3 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 3 * P2_z ** 2 + 3 * L ** 2 * P1_z ** 2 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z * P2_z ** 4 - 2 * P1_z ** 3 * P2_z ** 4) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4) - 2 * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) / (
+ L * sqrt(L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) * sqrt(
+ 4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * t.sin_t0)) / sqrt(1 - (
+ 4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * sin(
+ T_0) - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ L ** 2 * (
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2 * t.sin_t0_sq)),
+ -(2 * P1_z ** 2 * P2_z * sqrt(4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ L * sqrt(L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2 * t.sin_t0) + P2_z * sqrt(
+ 4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * sin(
+ T_0) - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ L * (L ** 2 - P2_z ** 2) ** (3 / 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) * t.sin_t0) + sqrt(
+ 4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ -L ** 3 * P1_z * t.sin_t0 - 2 * L ** 3 * P2_z * t.sin_t0 + 3 * L * P1_z * P2_z ** 2 * t.sin_t0 - P2_z * (
+ L ** 6 * P1_z * t.sin_t0_sq + 2 * L ** 6 * P2_z * t.sin_t0_sq - L ** 6 * P2_z - L ** 4 * P1_z ** 3 * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z + 3 * L ** 2 * P1_z ** 3 * P2_z ** 2 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 2 * P2_z ** 3 - 2 * P1_z ** 4 * P2_z ** 3) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4) - sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ L * sqrt(L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) * t.sin_t0) + (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ -8 * L ** 2 * P1_z ** 2 * P2_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 3) - 8 * L ** 2 * P1_z ** 2 * P2_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 3) - 4 * L ** 2 * P2_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) ** 2 * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 2 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ -2 * L ** 3 * P1_z * t.sin_t0 - 4 * L ** 3 * P2_z * t.sin_t0 + 6 * L * P1_z * P2_z ** 2 * t.sin_t0 - 2 * P2_z * (
+ L ** 6 * P1_z * t.sin_t0_sq + 2 * L ** 6 * P2_z * t.sin_t0_sq - L ** 6 * P2_z - L ** 4 * P1_z ** 3 * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z + 3 * L ** 2 * P1_z ** 3 * P2_z ** 2 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 2 * P2_z ** 3 - 2 * P1_z ** 4 * P2_z ** 3) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4) - 2 * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 2 * L ** 2 * (
+ -2 * L ** 3 * P1_z * t.sin_t0 + 2 * L * P1_z ** 3 * t.sin_t0 - 2 * P1_z * (
+ L ** 6 * P1_z * t.sin_t0_sq + 2 * L ** 6 * P2_z * t.sin_t0_sq - L ** 6 * P2_z - L ** 4 * P1_z ** 3 * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z + 3 * L ** 2 * P1_z ** 3 * P2_z ** 2 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 2 * P2_z ** 3 - 2 * P1_z ** 4 * P2_z ** 3) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) / (
+ L * sqrt(L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) * sqrt(
+ 4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * t.sin_t0)) / sqrt(1 - (
+ 4 * L ** 2 * t.sin_t0_sq - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 4 * L ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * sin(
+ T_0) - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * t.sin_t0_sq / (
+ (L ** 2 - P1_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ L ** 2 * (
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2 * t.sin_t0_sq))
+
+ ]
+ jac[5] = [
+
+ (1 - 2 * (L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * sin(
+ T_0) - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (-8 * P1_z * P2_z ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * sin(
+ T_0) - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ sqrt(
+ L ** 2 - P1_z ** 2) * sqrt(
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 3) - 2 * P1_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ (L ** 2 - P1_z ** 2) ** (
+ 3 / 2) * sqrt(
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 2 * (
+ -L ** 3 * P2_z * t.sin_t0 + L * P2_z ** 3 * t.sin_t0 - P2_z * (
+ 2 * L ** 6 * P1_z * t.sin_t0_sq - L ** 6 * P1_z + L ** 6 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq - L ** 4 * P2_z ** 3 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 3 * P2_z ** 2 + 3 * L ** 2 * P1_z ** 2 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z * P2_z ** 4 - 2 * P1_z ** 3 * P2_z ** 4) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ sqrt(
+ L ** 2 - P1_z ** 2) * sqrt(
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ -2 * L ** 3 * P1_z * t.sin_t0 - L ** 3 * P2_z * t.sin_t0 + 3 * L * P1_z ** 2 * P2_z * t.sin_t0 - P1_z * (
+ 2 * L ** 6 * P1_z * t.sin_t0_sq - L ** 6 * P1_z + L ** 6 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq - L ** 4 * P2_z ** 3 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 3 * P2_z ** 2 + 3 * L ** 2 * P1_z ** 2 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z * P2_z ** 4 - 2 * P1_z ** 3 * P2_z ** 4) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4) - sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ sqrt(
+ L ** 2 - P1_z ** 2) * sqrt(
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) / (
+ (1 - 2 * (L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * sin(
+ T_0) - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) ** 2 + 4 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * sin(
+ T_0) - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * sin(
+ T_0) - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P1_z ** 2) * (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 4)) + 2 * (
+ -8 * P1_z * P2_z ** 2 * (L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * sin(
+ T_0) - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 3) - 2 * (
+ -2 * L ** 3 * P2_z * t.sin_t0 + 2 * L * P2_z ** 3 * t.sin_t0 - 2 * P2_z * (
+ 2 * L ** 6 * P1_z * t.sin_t0_sq - L ** 6 * P1_z + L ** 6 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq - L ** 4 * P2_z ** 3 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 3 * P2_z ** 2 + 3 * L ** 2 * P1_z ** 2 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z * P2_z ** 4 - 2 * P1_z ** 3 * P2_z ** 4) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * sin(
+ T_0) - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * sin(
+ T_0) - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * sin(
+ T_0) - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ sqrt(L ** 2 - P1_z ** 2) * sqrt(L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2 * ((
+ 1 - 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) ** 2 + 4 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (
+ L ** 2 - P1_z ** 2) * (
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 4))),
+ (1 - 2 * (L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * sin(
+ T_0) - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (-8 * P1_z ** 2 * P2_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * sin(
+ T_0) - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ sqrt(
+ L ** 2 - P1_z ** 2) * sqrt(
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 3) - 2 * P2_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ sqrt(L ** 2 - P1_z ** 2) * (
+ L ** 2 - P2_z ** 2) ** (
+ 3 / 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 2 * (
+ -L ** 3 * P1_z * t.sin_t0 + L * P1_z ** 3 * t.sin_t0 - P1_z * (
+ L ** 6 * P1_z * t.sin_t0_sq + 2 * L ** 6 * P2_z * t.sin_t0_sq - L ** 6 * P2_z - L ** 4 * P1_z ** 3 * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z + 3 * L ** 2 * P1_z ** 3 * P2_z ** 2 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 2 * P2_z ** 3 - 2 * P1_z ** 4 * P2_z ** 3) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ sqrt(
+ L ** 2 - P1_z ** 2) * sqrt(
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ -L ** 3 * P1_z * t.sin_t0 - 2 * L ** 3 * P2_z * t.sin_t0 + 3 * L * P1_z * P2_z ** 2 * t.sin_t0 - P2_z * (
+ L ** 6 * P1_z * t.sin_t0_sq + 2 * L ** 6 * P2_z * t.sin_t0_sq - L ** 6 * P2_z - L ** 4 * P1_z ** 3 * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z + 3 * L ** 2 * P1_z ** 3 * P2_z ** 2 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 2 * P2_z ** 3 - 2 * P1_z ** 4 * P2_z ** 3) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4) - sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ sqrt(
+ L ** 2 - P1_z ** 2) * sqrt(
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) / (
+ (1 - 2 * (L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * sin(
+ T_0) - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) ** 2 + 4 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * sin(
+ T_0) - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * sin(
+ T_0) - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P1_z ** 2) * (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 4)) + 2 * (
+ -8 * P1_z ** 2 * P2_z * (L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * sin(
+ T_0) - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 3) - 4 * P2_z * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * sin(
+ T_0) - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (L ** 2 - P2_z ** 2) ** 2 * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2) - 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * sin(
+ T_0) - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ -2 * L ** 3 * P1_z * t.sin_t0 - 4 * L ** 3 * P2_z * sin(
+ T_0) + 6 * L * P1_z * P2_z ** 2 * t.sin_t0 - 2 * P2_z * (
+ L ** 6 * P1_z * t.sin_t0_sq + 2 * L ** 6 * P2_z * t.sin_t0_sq - L ** 6 * P2_z - L ** 4 * P1_z ** 3 * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z * t.sin_t0_sq - 3 * L ** 4 * P1_z * P2_z ** 2 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z + 3 * L ** 2 * P1_z ** 3 * P2_z ** 2 * t.sin_t0_sq + 2 * L ** 2 * P1_z ** 2 * P2_z ** 3 - 2 * P1_z ** 4 * P2_z ** 3) / sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4) - 2 * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ (L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * sin(
+ T_0) - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * sin(
+ T_0) - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) / (
+ sqrt(L ** 2 - P1_z ** 2) * sqrt(L ** 2 - P2_z ** 2) * (L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2 * ((
+ 1 - 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 2)) ** 2 + 4 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z ** 2 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 + L * P1_z ** 3 * P2_z * t.sin_t0 - P1_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 * (
+ L ** 5 * t.sin_t0 - L ** 3 * P1_z * P2_z * t.sin_t0 - L ** 3 * P2_z ** 2 * t.sin_t0 + L * P1_z * P2_z ** 3 * t.sin_t0 - P2_z * sqrt(
+ -2 * L ** 8 * t.sin_t0_sq + L ** 8 + 2 * L ** 6 * P1_z ** 2 * t.sin_t0_sq - L ** 6 * P1_z ** 2 + 2 * L ** 6 * P1_z * P2_z * t.sin_t0_sq + 2 * L ** 6 * P2_z ** 2 * t.sin_t0_sq - L ** 6 * P2_z ** 2 - 2 * L ** 4 * P1_z ** 3 * P2_z * t.sin_t0_sq - 2 * L ** 4 * P1_z ** 2 * P2_z ** 2 * t.sin_t0_sq - 2 * L ** 4 * P1_z * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 4 * P2_z ** 2 + 2 * L ** 2 * P1_z ** 3 * P2_z ** 3 * t.sin_t0_sq + L ** 2 * P1_z ** 2 * P2_z ** 4 - P1_z ** 4 * P2_z ** 4)) ** 2 / (
+ (
+ L ** 2 - P1_z ** 2) * (
+ L ** 2 - P2_z ** 2) * (
+ L ** 4 - P1_z ** 2 * P2_z ** 2) ** 4)))]
+ return jac
diff --git a/python/hemisphere-joint-demo/hemisphere-joint-demo/terms.py b/python/hemisphere-joint-demo/hemisphere-joint-demo/terms.py
new file mode 100755
index 000000000..96997e99e
--- /dev/null
+++ b/python/hemisphere-joint-demo/hemisphere-joint-demo/terms.py
@@ -0,0 +1,13 @@
+import numpy as np
+
+from numpy import pi, sin, cos, sqrt, arcsin, arctan2
+
+
+class CommonTerms:
+ def __init__(self, L, T_0):
+ self.sin_t0 = sin(T_0)
+ self.cos_t0 = cos(T_0)
+
+ self.sin_t0_sq = self.sin_t0 ** 2
+ self.cos_t0_sq = self.cos_t0 ** 2
+
--
GitLab