diff --git a/SimoxUtility/color/json.cpp b/SimoxUtility/color/json.cpp index 12bbf17e77fc6e084a061bfd1339a93182bfd14c..b7dedca39cdcb655b2b41b3222c747e109d27101 100644 --- a/SimoxUtility/color/json.cpp +++ b/SimoxUtility/color/json.cpp @@ -14,10 +14,10 @@ namespace simox void color::from_json(const nlohmann::json& j, Color& color) { - color.r = j.at("r"); - color.g = j.at("g"); - color.b = j.at("b"); - color.a = j.at("a"); + j.at("r").get_to(color.r); + j.at("g").get_to(color.g); + j.at("b").get_to(color.b); + j.at("a").get_to(color.a); } } diff --git a/SimoxUtility/xml/rapidxml/rapidxml_print.hpp b/SimoxUtility/xml/rapidxml/rapidxml_print.hpp index ceed57fac67dab7583d00c3bfe4b73f94423ccd4..a94aa5db7604a841c753d91c9ad564034f2b586b 100644 --- a/SimoxUtility/xml/rapidxml/rapidxml_print.hpp +++ b/SimoxUtility/xml/rapidxml/rapidxml_print.hpp @@ -248,6 +248,8 @@ namespace rapidxml template<class OutIt, class Ch> inline OutIt print_attributes(OutIt out, const xml_node<Ch>* node, int flags) { + (void) flags; // Unused. + for (xml_attribute<Ch>* attribute = node->first_attribute(); attribute; attribute = attribute->next_attribute()) { if (attribute->name() && attribute->value()) diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt index 04f11da66f22351d4b217b882b804233f2b31adc..d3c3363035ef3ca72c1ef13c97da3c7a0814c346 100644 --- a/VirtualRobot/CMakeLists.txt +++ b/VirtualRobot/CMakeLists.txt @@ -324,6 +324,8 @@ SET(SOURCES Nodes/PositionSensorFactory.cpp Nodes/RobotNode.cpp Nodes/RobotNodeActuator.cpp + Nodes/RobotNodeHemisphere.cpp + Nodes/RobotNodeHemisphereFactory.cpp Nodes/RobotNodeFixed.cpp Nodes/RobotNodeFixedFactory.cpp Nodes/RobotNodePrismatic.cpp @@ -331,6 +333,8 @@ SET(SOURCES Nodes/RobotNodeRevolute.cpp Nodes/RobotNodeRevoluteFactory.cpp Nodes/Sensor.cpp + Nodes/HemisphereJoint/Expressions.cpp + Nodes/HemisphereJoint/Joint.cpp TimeOptimalTrajectory/Path.cpp TimeOptimalTrajectory/TimeOptimalTrajectory.cpp @@ -561,6 +565,8 @@ SET(INCLUDES Nodes/PositionSensorFactory.h Nodes/RobotNode.h Nodes/RobotNodeActuator.h + Nodes/RobotNodeHemisphere.h + Nodes/RobotNodeHemisphereFactory.h Nodes/RobotNodeFactory.h Nodes/RobotNodeFixed.h Nodes/RobotNodeFixedFactory.h @@ -570,6 +576,8 @@ SET(INCLUDES Nodes/RobotNodeRevoluteFactory.h Nodes/Sensor.h Nodes/SensorFactory.h + Nodes/HemisphereJoint/Expressions.h + Nodes/HemisphereJoint/Joint.h TimeOptimalTrajectory/Path.h TimeOptimalTrajectory/TimeOptimalTrajectory.h diff --git a/VirtualRobot/MathTools.cpp b/VirtualRobot/MathTools.cpp index 92be5ef33b3f1e6cddc028e5c44ba2a0b82bc58e..f8cdaa2afaaac06aa2890901d7416a9dd97fd159 100644 --- a/VirtualRobot/MathTools.cpp +++ b/VirtualRobot/MathTools.cpp @@ -14,6 +14,9 @@ #include <Eigen/Geometry> #define MIN_TO_ZERO (1e-6f) +#ifndef M_PIf +#define M_PIf (static_cast<float>(M_PI)) +#endif namespace VirtualRobot { diff --git a/VirtualRobot/Nodes/HemisphereJoint/Expressions.cpp b/VirtualRobot/Nodes/HemisphereJoint/Expressions.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7aea99f1558a4de8934621b02a2635df18cd98b4 --- /dev/null +++ b/VirtualRobot/Nodes/HemisphereJoint/Expressions.cpp @@ -0,0 +1,246 @@ +/* + * This file was generated automatically on 2022-06-09 10:41. + */ + +#include "Expressions.h" + +#include <cmath> + + +namespace VirtualRobot::hemisphere +{ + +void Expressions::compute(double a1, double a2, double lever, double theta0) +{ + this->a1 = a1; + this->a2 = a2; + this->lever = lever; + this->theta0 = theta0; + + _lever_p_2 = (lever * lever); + _a2_p_2 = (a2 * a2); + __s_a2_p_2 = (-1 * _a2_p_2); + __s_a2_p_2_a_lever_p_2 = (_lever_p_2 + __s_a2_p_2); + _1_d_sqrt_l__s_a2_p_2_a_lever_p_2_r_ = std::pow(__s_a2_p_2_a_lever_p_2, -0.5); + _lever_p_4 = std::pow(lever, 4); + _a1_p_2 = (a1 * a1); + __s_a1_p_2_m_a2_p_2 = (-1 * _a1_p_2 * _a2_p_2); + __s_a1_p_2_m_a2_p_2_a_lever_p_4 = (_lever_p_4 + __s_a1_p_2_m_a2_p_2); + _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ = (1 / __s_a1_p_2_m_a2_p_2_a_lever_p_4); + _lever_p_5 = std::pow(lever, 5); + _sin_l_theta0_r_ = std::sin(theta0); + _lever_p_5_m_sin_l_theta0_r_ = (_lever_p_5 * _sin_l_theta0_r_); + _lever_p_8 = std::pow(lever, 8); + _lever_p_6 = std::pow(lever, 6); + __s_a1_p_2_m_lever_p_6 = (-1 * _a1_p_2 * _lever_p_6); + _a1_p_4 = std::pow(a1, 4); + _a2_p_4 = std::pow(a2, 4); + __s_a1_p_4_m_a2_p_4 = (-1 * _a1_p_4 * _a2_p_4); + __s_a2_p_2_m_lever_p_6 = (-1 * _a2_p_2 * _lever_p_6); + _sin_l_theta0_r__p_2 = (_sin_l_theta0_r_ * _sin_l_theta0_r_); + __s_2_m_lever_p_8_m_sin_l_theta0_r__p_2 = (-2 * _lever_p_8 * _sin_l_theta0_r__p_2); + _a1_p_2_m_a2_p_4_m_lever_p_2 = (_a1_p_2 * _a2_p_4 * _lever_p_2); + _a1_p_4_m_a2_p_2_m_lever_p_2 = (_a1_p_4 * _a2_p_2 * _lever_p_2); + _2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2 = (2 * _a1_p_2 * _lever_p_6 * _sin_l_theta0_r__p_2); + _2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2 = (2 * _a2_p_2 * _lever_p_6 * _sin_l_theta0_r__p_2); + _a2_p_3 = std::pow(a2, 3); + __s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2 = (-2 * a1 * _a2_p_3 * _lever_p_4 * _sin_l_theta0_r__p_2); + _a1_p_3 = std::pow(a1, 3); + __s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2 = (-2 * a2 * _a1_p_3 * _lever_p_4 * _sin_l_theta0_r__p_2); + __s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2 = (-2 * _a1_p_2 * _a2_p_2 * _lever_p_4 * _sin_l_theta0_r__p_2); + _2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2 = (2 * a1 * a2 * _lever_p_6 * _sin_l_theta0_r__p_2); + _2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2 = (2 * _a1_p_3 * _a2_p_3 * _lever_p_2 * _sin_l_theta0_r__p_2); + __s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8 = (_lever_p_8 + __s_a1_p_2_m_lever_p_6 + __s_a1_p_4_m_a2_p_4 + __s_a2_p_2_m_lever_p_6 + __s_2_m_lever_p_8_m_sin_l_theta0_r__p_2 + _a1_p_2_m_a2_p_4_m_lever_p_2 + _a1_p_4_m_a2_p_2_m_lever_p_2 + _2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2 + _2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2 + __s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2 + __s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2 + __s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2 + _2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2 + _2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2); + _sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = std::pow(__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8, 0.5); + __s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = (-1 * a2 * _sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_); + _lever_p_3 = std::pow(lever, 3); + __s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r_ = (-1 * _a2_p_2 * _lever_p_3 * _sin_l_theta0_r_); + _a1_m_a2_p_3_m_lever_m_sin_l_theta0_r_ = (a1 * lever * _a2_p_3 * _sin_l_theta0_r_); + __s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r_ = (-1 * a1 * a2 * _lever_p_3 * _sin_l_theta0_r_); + _a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_ = (_lever_p_5_m_sin_l_theta0_r_ + __s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ + __s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r_ + _a1_m_a2_p_3_m_lever_m_sin_l_theta0_r_ + __s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r_); + _2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = (2 * lever * _1_d_sqrt_l__s_a2_p_2_a_lever_p_2_r_ * _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ * _a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_ * _sin_l_theta0_r_); + __s_a1_p_2 = (-1 * _a1_p_2); + __s_a1_p_2_a_lever_p_2 = (_lever_p_2 + __s_a1_p_2); + _1_d_sqrt_l__s_a1_p_2_a_lever_p_2_r_ = std::pow(__s_a1_p_2_a_lever_p_2, -0.5); + __s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = (-1 * a1 * _sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_); + __s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r_ = (-1 * _a1_p_2 * _lever_p_3 * _sin_l_theta0_r_); + _a1_p_3_m_a2_m_lever_m_sin_l_theta0_r_ = (a2 * lever * _a1_p_3 * _sin_l_theta0_r_); + _a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_ = (_lever_p_5_m_sin_l_theta0_r_ + __s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ + __s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r_ + _a1_p_3_m_a2_m_lever_m_sin_l_theta0_r_ + __s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r_); + _2_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = (2 * lever * _1_d_sqrt_l__s_a1_p_2_a_lever_p_2_r_ * _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ * _a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_ * _sin_l_theta0_r_); + _a1_m_a2 = (a1 * a2); + _a1_m_a2_a_lever_p_2 = (_lever_p_2 + _a1_m_a2); + _1_d__l_a1_m_a2_a_lever_p_2_r_ = (1 / _a1_m_a2_a_lever_p_2); + __s_lever_p_4 = (-1 * _lever_p_4); + _a1_p_2_m_a2_p_2 = (_a1_p_2 * _a2_p_2); + _a1_p_2_m_a2_p_2_s_lever_p_4 = (__s_lever_p_4 + _a1_p_2_m_a2_p_2); + _cos_l_theta0_r_ = std::cos(theta0); + _cos_l_theta0_r__p_2 = (_cos_l_theta0_r_ * _cos_l_theta0_r_); + __s_2_m_lever_p_4_m_cos_l_theta0_r__p_2 = (-2 * _lever_p_4 * _cos_l_theta0_r__p_2); + _a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2 = (_a1_p_2 * _lever_p_2 * _cos_l_theta0_r__p_2); + _a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2 = (_a2_p_2 * _lever_p_2 * _cos_l_theta0_r__p_2); + __s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4 = (_lever_p_4 + __s_a1_p_2_m_a2_p_2 + __s_2_m_lever_p_4_m_cos_l_theta0_r__p_2 + _a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2 + _a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2); + __l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r_ = (_a1_p_2_m_a2_p_2_s_lever_p_4 * __s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4); + _a1_m_lever_p_2 = (a1 * _lever_p_2); + _a2_m_lever_p_2 = (a2 * _lever_p_2); + __s_a1_m_a2_p_2 = (-1 * a1 * _a2_p_2); + __s_a1_p_2_m_a2 = (-1 * a2 * _a1_p_2); + __s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2 = (_a1_m_lever_p_2 + _a2_m_lever_p_2 + __s_a1_m_a2_p_2 + __s_a1_p_2_m_a2); + __l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2 = (__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2 * __s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2); + _lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2 = (_lever_p_2 * __l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2 * _sin_l_theta0_r__p_2); + _lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r_ = (__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r_ + _lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2); + _sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = std::pow(_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r_, 0.5); + __l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = (_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ * _a1_m_a2_a_lever_p_2); + _a1_a_a2 = (a1 + a2); + _lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r_ = (lever * _a1_a_a2 * __s_a1_p_2_m_a2_p_2_a_lever_p_4 * _sin_l_theta0_r_); + _lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = (__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ + _lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r_); + _2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = (2 * lever * _1_d__l_a1_m_a2_a_lever_p_2_r_ * _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ * _lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ * _sin_l_theta0_r_); + _1_d__l__s_a2_p_2_a_lever_p_2_r_ = (1 / __s_a2_p_2_a_lever_p_2); + __l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p__l__s_2_r_ = std::pow(__s_a1_p_2_m_a2_p_2_a_lever_p_4, -2); + __l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2 = (_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_ * _a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_); + __s_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = (-2 * _1_d__l__s_a2_p_2_a_lever_p_2_r_ * __l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p__l__s_2_r_ * __l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2); + _1_s_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = (1 + __s_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_); + __s_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = (-2 * _1_d_sqrt_l__s_a1_p_2_a_lever_p_2_r_ * _1_d_sqrt_l__s_a2_p_2_a_lever_p_2_r_ * __l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p__l__s_2_r_ * _a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_ * _a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_); + _1_d_lever = (1 / lever); + _4_m_lever_p_2_m_sin_l_theta0_r__p_2 = (4 * _lever_p_2 * _sin_l_theta0_r__p_2); + _1_d__l__s_a1_p_2_a_lever_p_2_r_ = (1 / __s_a1_p_2_a_lever_p_2); + __l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2 = (_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_ * _a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_); + __s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = (-4 * _lever_p_2 * _1_d__l__s_a1_p_2_a_lever_p_2_r_ * __l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p__l__s_2_r_ * __l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2 * _sin_l_theta0_r__p_2); + __s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = (-4 * _lever_p_2 * _1_d__l__s_a2_p_2_a_lever_p_2_r_ * __l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p__l__s_2_r_ * __l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2 * _sin_l_theta0_r__p_2); + _4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = (_4_m_lever_p_2_m_sin_l_theta0_r__p_2 + __s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ + __s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_); + _sqrt_l_4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r_ = std::pow(_4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_, 0.5); + _1_d_sin_l_theta0_r_ = (1 / _sin_l_theta0_r_); + __s_sqrt_l_4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r__m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__d__l_lever_m_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__r_ = (-1 * _1_d_lever * _1_d_sqrt_l__s_a2_p_2_a_lever_p_2_r_ * _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ * _sqrt_l_4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r_ * _1_d_sin_l_theta0_r_ * _a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_); + __s_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = (-2 * _1_d__l__s_a1_p_2_a_lever_p_2_r_ * __l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p__l__s_2_r_ * __l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2); + _1_s_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = (1 + __s_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_); + __s_sqrt_l_4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r__m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__d__l_lever_m_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__r_ = (-1 * _1_d_lever * _1_d_sqrt_l__s_a1_p_2_a_lever_p_2_r_ * _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ * _sqrt_l_4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r_ * _1_d_sin_l_theta0_r_ * _a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_); + _sqrt_l_4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r__m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__d__l_lever_m_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__r_ = (_1_d_lever * _1_d_sqrt_l__s_a2_p_2_a_lever_p_2_r_ * _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ * _sqrt_l_4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r_ * _1_d_sin_l_theta0_r_ * _a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_); + _sqrt_l_4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r__m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__d__l_lever_m_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__r_ = (_1_d_lever * _1_d_sqrt_l__s_a1_p_2_a_lever_p_2_r_ * _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ * _sqrt_l_4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r_ * _1_d_sin_l_theta0_r_ * _a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_); + _lever_p__l__s_2_r_ = std::pow(lever, -2); + _2_m_lever_p_2 = (2 * _lever_p_2); + __s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = (-4 * _lever_p_2 * _1_d__l__s_a1_p_2_a_lever_p_2_r_ * __l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p__l__s_2_r_ * __l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2); + __s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = (-4 * _lever_p_2 * _1_d__l__s_a2_p_2_a_lever_p_2_r_ * __l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p__l__s_2_r_ * __l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2); + _2_m_lever_p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = (_2_m_lever_p_2 + __s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ + __s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_); + __l_2_m_lever_p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r__d__l_2_m_lever_p_2_r_ = (0.5 * _lever_p__l__s_2_r_ * _2_m_lever_p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_); + _a2_p_3_m_lever_m_sin_l_theta0_r_ = (lever * _a2_p_3 * _sin_l_theta0_r_); + __s_a2_m_lever_p_3_m_sin_l_theta0_r_ = (-1 * a2 * _lever_p_3 * _sin_l_theta0_r_); + _1_d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = std::pow(__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8, -0.5); + __s_a1_m_lever_p_6 = (-1 * a1 * _lever_p_6); + __s_2_m_a1_p_3_m_a2_p_4 = (-2 * _a1_p_3 * _a2_p_4); + _a1_m_a2_p_4_m_lever_p_2 = (a1 * _a2_p_4 * _lever_p_2); + _a2_m_lever_p_6_m_sin_l_theta0_r__p_2 = (a2 * _lever_p_6 * _sin_l_theta0_r__p_2); + __s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2 = (-1 * _a2_p_3 * _lever_p_4 * _sin_l_theta0_r__p_2); + _2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2 = (2 * a1 * _lever_p_6 * _sin_l_theta0_r__p_2); + _2_m_a1_p_3_m_a2_p_2_m_lever_p_2 = (2 * _a1_p_3 * _a2_p_2 * _lever_p_2); + __s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2 = (-3 * a2 * _a1_p_2 * _lever_p_4 * _sin_l_theta0_r__p_2); + __s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2 = (-2 * a1 * _a2_p_2 * _lever_p_4 * _sin_l_theta0_r__p_2); + _3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2 = (3 * _a1_p_2 * _a2_p_3 * _lever_p_2 * _sin_l_theta0_r__p_2); + __s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2 = (__s_a1_m_lever_p_6 + __s_2_m_a1_p_3_m_a2_p_4 + _a1_m_a2_p_4_m_lever_p_2 + _a2_m_lever_p_6_m_sin_l_theta0_r__p_2 + __s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2 + _2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2 + _2_m_a1_p_3_m_a2_p_2_m_lever_p_2 + __s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2 + __s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2 + _3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2); + __s_a2_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = (-1 * a2 * _1_d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ * __s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2); + _a2_p_3_m_lever_m_sin_l_theta0_r__s_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = (_a2_p_3_m_lever_m_sin_l_theta0_r_ + __s_a2_m_lever_p_3_m_sin_l_theta0_r_ + __s_a2_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_); + _2_m_lever_m__l_a2_p_3_m_lever_m_sin_l_theta0_r__s_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = (2 * lever * _1_d_sqrt_l__s_a2_p_2_a_lever_p_2_r_ * _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ * _a2_p_3_m_lever_m_sin_l_theta0_r__s_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ * _sin_l_theta0_r_); + _4_m_a1_m_a2_p_2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = (4 * a1 * lever * _a2_p_2 * _1_d_sqrt_l__s_a2_p_2_a_lever_p_2_r_ * __l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p__l__s_2_r_ * _a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_ * _sin_l_theta0_r_); + _4_m_a1_m_a2_p_2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__a_2_m_lever_m__l_a2_p_3_m_lever_m_sin_l_theta0_r__s_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = (_2_m_lever_m__l_a2_p_3_m_lever_m_sin_l_theta0_r__s_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ + _4_m_a1_m_a2_p_2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_); + __s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = (-1 * _sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_); + __s_a1_m_lever_p_3_m_sin_l_theta0_r_ = (-1 * a1 * _lever_p_3 * _sin_l_theta0_r_); + __s_a2_m_lever_p_6 = (-1 * a2 * _lever_p_6); + __s_2_m_a1_p_4_m_a2_p_3 = (-2 * _a1_p_4 * _a2_p_3); + _a1_m_lever_p_6_m_sin_l_theta0_r__p_2 = (a1 * _lever_p_6 * _sin_l_theta0_r__p_2); + _a1_p_4_m_a2_m_lever_p_2 = (a2 * _a1_p_4 * _lever_p_2); + __s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2 = (-1 * _a1_p_3 * _lever_p_4 * _sin_l_theta0_r__p_2); + _2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2 = (2 * a2 * _lever_p_6 * _sin_l_theta0_r__p_2); + _2_m_a1_p_2_m_a2_p_3_m_lever_p_2 = (2 * _a1_p_2 * _a2_p_3 * _lever_p_2); + __s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2 = (-3 * a1 * _a2_p_2 * _lever_p_4 * _sin_l_theta0_r__p_2); + __s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2 = (-2 * a2 * _a1_p_2 * _lever_p_4 * _sin_l_theta0_r__p_2); + _3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2 = (3 * _a1_p_3 * _a2_p_2 * _lever_p_2 * _sin_l_theta0_r__p_2); + __s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6 = (__s_a2_m_lever_p_6 + __s_2_m_a1_p_4_m_a2_p_3 + _a1_m_lever_p_6_m_sin_l_theta0_r__p_2 + _a1_p_4_m_a2_m_lever_p_2 + __s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2 + _2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2 + _2_m_a1_p_2_m_a2_p_3_m_lever_p_2 + __s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2 + __s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2 + _3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2); + __s_a2_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = (-1 * a2 * _1_d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ * __s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6); + __s_2_m_a2_m_lever_p_3_m_sin_l_theta0_r_ = (-2 * a2 * _lever_p_3 * _sin_l_theta0_r_); + _3_m_a1_m_a2_p_2_m_lever_m_sin_l_theta0_r_ = (3 * a1 * lever * _a2_p_2 * _sin_l_theta0_r_); + _3_m_a1_m_a2_p_2_m_lever_m_sin_l_theta0_r__s_a1_m_lever_p_3_m_sin_l_theta0_r__s_2_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = (__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ + __s_a1_m_lever_p_3_m_sin_l_theta0_r_ + __s_a2_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ + __s_2_m_a2_m_lever_p_3_m_sin_l_theta0_r_ + _3_m_a1_m_a2_p_2_m_lever_m_sin_l_theta0_r_); + _2_m_lever_m__l_3_m_a1_m_a2_p_2_m_lever_m_sin_l_theta0_r__s_a1_m_lever_p_3_m_sin_l_theta0_r__s_2_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = (2 * lever * _1_d_sqrt_l__s_a2_p_2_a_lever_p_2_r_ * _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ * _3_m_a1_m_a2_p_2_m_lever_m_sin_l_theta0_r__s_a1_m_lever_p_3_m_sin_l_theta0_r__s_2_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ * _sin_l_theta0_r_); + __l__s_a2_p_2_a_lever_p_2_r__p__l__s_1_t_5_r_ = std::pow(__s_a2_p_2_a_lever_p_2, -1.5); + _2_m_a2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l__l__s_a2_p_2_a_lever_p_2_r__p_1_t_5_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = (2 * a2 * lever * __l__s_a2_p_2_a_lever_p_2_r__p__l__s_1_t_5_r_ * _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ * _a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_ * _sin_l_theta0_r_); + _4_m_a1_p_2_m_a2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = (4 * a2 * lever * _a1_p_2 * _1_d_sqrt_l__s_a2_p_2_a_lever_p_2_r_ * __l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p__l__s_2_r_ * _a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_ * _sin_l_theta0_r_); + _4_m_a1_p_2_m_a2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__a_2_m_a2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l__l__s_a2_p_2_a_lever_p_2_r__p_1_t_5_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r__a_2_m_lever_m__l_3_m_a1_m_a2_p_2_m_lever_m_sin_l_theta0_r__s_a1_m_lever_p_3_m_sin_l_theta0_r__s_2_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = (_2_m_lever_m__l_3_m_a1_m_a2_p_2_m_lever_m_sin_l_theta0_r__s_a1_m_lever_p_3_m_sin_l_theta0_r__s_2_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ + _2_m_a2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l__l__s_a2_p_2_a_lever_p_2_r__p_1_t_5_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ + _4_m_a1_p_2_m_a2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_); + __s_a1_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = (-1 * a1 * _1_d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ * __s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2); + __s_2_m_a1_m_lever_p_3_m_sin_l_theta0_r_ = (-2 * a1 * _lever_p_3 * _sin_l_theta0_r_); + _3_m_a1_p_2_m_a2_m_lever_m_sin_l_theta0_r_ = (3 * a2 * lever * _a1_p_2 * _sin_l_theta0_r_); + _3_m_a1_p_2_m_a2_m_lever_m_sin_l_theta0_r__s_2_m_a1_m_lever_p_3_m_sin_l_theta0_r__s_a1_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__s_a2_m_lever_p_3_m_sin_l_theta0_r__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = (__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ + __s_a1_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ + __s_a2_m_lever_p_3_m_sin_l_theta0_r_ + __s_2_m_a1_m_lever_p_3_m_sin_l_theta0_r_ + _3_m_a1_p_2_m_a2_m_lever_m_sin_l_theta0_r_); + _2_m_lever_m__l_3_m_a1_p_2_m_a2_m_lever_m_sin_l_theta0_r__s_2_m_a1_m_lever_p_3_m_sin_l_theta0_r__s_a1_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__s_a2_m_lever_p_3_m_sin_l_theta0_r__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = (2 * lever * _1_d_sqrt_l__s_a1_p_2_a_lever_p_2_r_ * _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ * _3_m_a1_p_2_m_a2_m_lever_m_sin_l_theta0_r__s_2_m_a1_m_lever_p_3_m_sin_l_theta0_r__s_a1_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__s_a2_m_lever_p_3_m_sin_l_theta0_r__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ * _sin_l_theta0_r_); + __l__s_a1_p_2_a_lever_p_2_r__p__l__s_1_t_5_r_ = std::pow(__s_a1_p_2_a_lever_p_2, -1.5); + _2_m_a1_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l__l__s_a1_p_2_a_lever_p_2_r__p_1_t_5_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = (2 * a1 * lever * __l__s_a1_p_2_a_lever_p_2_r__p__l__s_1_t_5_r_ * _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ * _a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_ * _sin_l_theta0_r_); + _4_m_a1_m_a2_p_2_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = (4 * a1 * lever * _a2_p_2 * _1_d_sqrt_l__s_a1_p_2_a_lever_p_2_r_ * __l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p__l__s_2_r_ * _a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_ * _sin_l_theta0_r_); + _4_m_a1_m_a2_p_2_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__a_2_m_a1_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l__l__s_a1_p_2_a_lever_p_2_r__p_1_t_5_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r__a_2_m_lever_m__l_3_m_a1_p_2_m_a2_m_lever_m_sin_l_theta0_r__s_2_m_a1_m_lever_p_3_m_sin_l_theta0_r__s_a1_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__s_a2_m_lever_p_3_m_sin_l_theta0_r__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = (_2_m_lever_m__l_3_m_a1_p_2_m_a2_m_lever_m_sin_l_theta0_r__s_2_m_a1_m_lever_p_3_m_sin_l_theta0_r__s_a1_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__s_a2_m_lever_p_3_m_sin_l_theta0_r__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ + _2_m_a1_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l__l__s_a1_p_2_a_lever_p_2_r__p_1_t_5_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ + _4_m_a1_m_a2_p_2_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_); + _a1_p_3_m_lever_m_sin_l_theta0_r_ = (lever * _a1_p_3 * _sin_l_theta0_r_); + __s_a1_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = (-1 * a1 * _1_d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ * __s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6); + _a1_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_lever_p_3_m_sin_l_theta0_r__s_a1_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = (_a1_p_3_m_lever_m_sin_l_theta0_r_ + __s_a1_m_lever_p_3_m_sin_l_theta0_r_ + __s_a1_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_); + _2_m_lever_m__l_a1_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_lever_p_3_m_sin_l_theta0_r__s_a1_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = (2 * lever * _1_d_sqrt_l__s_a1_p_2_a_lever_p_2_r_ * _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ * _a1_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_lever_p_3_m_sin_l_theta0_r__s_a1_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ * _sin_l_theta0_r_); + _4_m_a1_p_2_m_a2_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = (4 * a2 * lever * _a1_p_2 * _1_d_sqrt_l__s_a1_p_2_a_lever_p_2_r_ * __l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p__l__s_2_r_ * _a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_ * _sin_l_theta0_r_); + _4_m_a1_p_2_m_a2_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__a_2_m_lever_m__l_a1_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_lever_p_3_m_sin_l_theta0_r__s_a1_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = (_2_m_lever_m__l_a1_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_lever_p_3_m_sin_l_theta0_r__s_a1_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ + _4_m_a1_p_2_m_a2_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_); + _a2_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = (a2 * _sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_); + _lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r_ = (lever * __s_a1_p_2_m_a2_p_2_a_lever_p_4 * _sin_l_theta0_r_); + _1_d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = std::pow(_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r_, -0.5); + __s_2_m_a1_m_a2_p_2 = (-2 * a1 * _a2_p_2); + _2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2 = (2 * a1 * _lever_p_2 * _cos_l_theta0_r__p_2); + __s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2 = (__s_2_m_a1_m_a2_p_2 + _2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2); + __l__s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2 = (0.5 * _a1_p_2_m_a2_p_2_s_lever_p_4 * __s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2); + _a1_m_a2_p_2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r_ = (a1 * _a2_p_2 * __s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4); + __s_2_m_a2_p_2 = (-2 * _a2_p_2); + __s_4_m_a1_m_a2 = (-4 * a1 * a2); + __s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2 = (__s_2_m_a2_p_2 + _2_m_lever_p_2 + __s_4_m_a1_m_a2); + _lever_p_2_m__l__s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2 = (0.5 * _lever_p_2 * _sin_l_theta0_r__p_2 * __s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2 * __s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2); + _a1_m_a2_p_2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2 = (__l__s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2 + _a1_m_a2_p_2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r_ + _lever_p_2_m__l__s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2); + __l_a1_m_a2_a_lever_p_2_r__m__l_a1_m_a2_p_2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = (_1_d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ * _a1_m_a2_a_lever_p_2 * _a1_m_a2_p_2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2); + __s_2_m_a1_m_a2_p_2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r_ = (-2 * a1 * lever * _a2_p_2 * _a1_a_a2 * _sin_l_theta0_r_); + __s_2_m_a1_m_a2_p_2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r__a_a2_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__a_lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m__l_a1_m_a2_p_2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = (_a2_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ + _lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r_ + __l_a1_m_a2_a_lever_p_2_r__m__l_a1_m_a2_p_2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ + __s_2_m_a1_m_a2_p_2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r_); + _2_m_lever_m__l__s_2_m_a1_m_a2_p_2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r__a_a2_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__a_lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m__l_a1_m_a2_p_2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = (2 * lever * _1_d__l_a1_m_a2_a_lever_p_2_r_ * _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ * __s_2_m_a1_m_a2_p_2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r__a_a2_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__a_lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m__l_a1_m_a2_p_2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ * _sin_l_theta0_r_); + __l_a1_m_a2_a_lever_p_2_r__p__l__s_2_r_ = std::pow(_a1_m_a2_a_lever_p_2, -2); + __s_2_m_a2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__p_2_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = (-2 * a2 * lever * __l_a1_m_a2_a_lever_p_2_r__p__l__s_2_r_ * _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ * _lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ * _sin_l_theta0_r_); + _4_m_a1_m_a2_p_2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = (4 * a1 * lever * _a2_p_2 * _1_d__l_a1_m_a2_a_lever_p_2_r_ * __l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p__l__s_2_r_ * _lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ * _sin_l_theta0_r_); + _4_m_a1_m_a2_p_2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_2_m_a2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__p_2_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r__a_2_m_lever_m__l__s_2_m_a1_m_a2_p_2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r__a_a2_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__a_lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m__l_a1_m_a2_p_2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = (_2_m_lever_m__l__s_2_m_a1_m_a2_p_2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r__a_a2_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__a_lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m__l_a1_m_a2_p_2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ + __s_2_m_a2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__p_2_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ + _4_m_a1_m_a2_p_2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_); + _a1_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = (a1 * _sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_); + __s_2_m_a1_p_2_m_a2 = (-2 * a2 * _a1_p_2); + _2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2 = (2 * a2 * _lever_p_2 * _cos_l_theta0_r__p_2); + __s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2 = (__s_2_m_a1_p_2_m_a2 + _2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2); + __l__s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2 = (0.5 * _a1_p_2_m_a2_p_2_s_lever_p_4 * __s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2); + _a1_p_2_m_a2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r_ = (a2 * _a1_p_2 * __s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4); + __s_2_m_a1_p_2 = (-2 * _a1_p_2); + __s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2 = (__s_2_m_a1_p_2 + _2_m_lever_p_2 + __s_4_m_a1_m_a2); + _lever_p_2_m__l__s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2 = (0.5 * _lever_p_2 * _sin_l_theta0_r__p_2 * __s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2 * __s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2); + _a1_p_2_m_a2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2 = (__l__s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2 + _a1_p_2_m_a2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r_ + _lever_p_2_m__l__s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2); + __l_a1_m_a2_a_lever_p_2_r__m__l_a1_p_2_m_a2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = (_1_d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ * _a1_m_a2_a_lever_p_2 * _a1_p_2_m_a2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2); + __s_2_m_a1_p_2_m_a2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r_ = (-2 * a2 * lever * _a1_p_2 * _a1_a_a2 * _sin_l_theta0_r_); + __s_2_m_a1_p_2_m_a2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r__a_a1_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__a_lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m__l_a1_p_2_m_a2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = (_a1_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ + _lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r_ + __l_a1_m_a2_a_lever_p_2_r__m__l_a1_p_2_m_a2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ + __s_2_m_a1_p_2_m_a2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r_); + _2_m_lever_m__l__s_2_m_a1_p_2_m_a2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r__a_a1_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__a_lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m__l_a1_p_2_m_a2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = (2 * lever * _1_d__l_a1_m_a2_a_lever_p_2_r_ * _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ * __s_2_m_a1_p_2_m_a2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r__a_a1_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__a_lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m__l_a1_p_2_m_a2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ * _sin_l_theta0_r_); + __s_2_m_a1_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__p_2_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = (-2 * a1 * lever * __l_a1_m_a2_a_lever_p_2_r__p__l__s_2_r_ * _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ * _lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ * _sin_l_theta0_r_); + _4_m_a1_p_2_m_a2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = (4 * a2 * lever * _a1_p_2 * _1_d__l_a1_m_a2_a_lever_p_2_r_ * __l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p__l__s_2_r_ * _lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ * _sin_l_theta0_r_); + _4_m_a1_p_2_m_a2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_2_m_a1_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__p_2_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r__a_2_m_lever_m__l__s_2_m_a1_p_2_m_a2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r__a_a1_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__a_lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m__l_a1_p_2_m_a2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = (_2_m_lever_m__l__s_2_m_a1_p_2_m_a2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r__a_a1_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__a_lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m__l_a1_p_2_m_a2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ + __s_2_m_a1_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__p_2_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ + _4_m_a1_p_2_m_a2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_); + + ex = _2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_; + ey = _2_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_; + ez = _2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_; + exx = _1_s_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_; + exy = __s_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_; + exz = __s_sqrt_l_4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r__m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__d__l_lever_m_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__r_; + eyx = __s_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_; + eyy = _1_s_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_; + eyz = __s_sqrt_l_4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r__m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__d__l_lever_m_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__r_; + ezx = _sqrt_l_4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r__m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__d__l_lever_m_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__r_; + ezy = _sqrt_l_4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r__m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__d__l_lever_m_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__r_; + ezz = __l_2_m_lever_p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r__d__l_2_m_lever_p_2_r_; + jx1 = _4_m_a1_m_a2_p_2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__a_2_m_lever_m__l_a2_p_3_m_lever_m_sin_l_theta0_r__s_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_; + jx2 = _4_m_a1_p_2_m_a2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__a_2_m_a2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l__l__s_a2_p_2_a_lever_p_2_r__p_1_t_5_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r__a_2_m_lever_m__l_3_m_a1_m_a2_p_2_m_lever_m_sin_l_theta0_r__s_a1_m_lever_p_3_m_sin_l_theta0_r__s_2_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_; + jy1 = _4_m_a1_m_a2_p_2_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__a_2_m_a1_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l__l__s_a1_p_2_a_lever_p_2_r__p_1_t_5_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r__a_2_m_lever_m__l_3_m_a1_p_2_m_a2_m_lever_m_sin_l_theta0_r__s_2_m_a1_m_lever_p_3_m_sin_l_theta0_r__s_a1_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__s_a2_m_lever_p_3_m_sin_l_theta0_r__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_; + jy2 = _4_m_a1_p_2_m_a2_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__a_2_m_lever_m__l_a1_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_lever_p_3_m_sin_l_theta0_r__s_a1_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_; + jz1 = _4_m_a1_m_a2_p_2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_2_m_a2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__p_2_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r__a_2_m_lever_m__l__s_2_m_a1_m_a2_p_2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r__a_a2_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__a_lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m__l_a1_m_a2_p_2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_; + jz2 = _4_m_a1_p_2_m_a2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_2_m_a1_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__p_2_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r__a_2_m_lever_m__l__s_2_m_a1_p_2_m_a2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r__a_a1_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__a_lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m__l_a1_p_2_m_a2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_; + jrx1 = _lever_p_2; + jrx2 = _lever_p_2; + jry1 = _lever_p_2; + jry2 = _lever_p_2; + jrz1 = _lever_p_2; + jrz2 = _lever_p_2; +} + +} + diff --git a/VirtualRobot/Nodes/HemisphereJoint/Expressions.h b/VirtualRobot/Nodes/HemisphereJoint/Expressions.h new file mode 100644 index 0000000000000000000000000000000000000000..94d95692abb333d4b87e59aacbc6427f29e1594d --- /dev/null +++ b/VirtualRobot/Nodes/HemisphereJoint/Expressions.h @@ -0,0 +1,252 @@ +/* + * This file was generated automatically on 2022-06-09 10:41. + */ + +#pragma once + + +namespace VirtualRobot::hemisphere +{ + +class Expressions +{ +public: + + void compute(double a1, double a2, double lever, double theta0); + + // Input arguments: + double a1 = 0; + double a2 = 0; + double lever = 0; + double theta0 = 0; + + // Results: + double ex = 0; + double ey = 0; + double ez = 0; + double exx = 0; + double exy = 0; + double exz = 0; + double eyx = 0; + double eyy = 0; + double eyz = 0; + double ezx = 0; + double ezy = 0; + double ezz = 0; + double jx1 = 0; + double jx2 = 0; + double jy1 = 0; + double jy2 = 0; + double jz1 = 0; + double jz2 = 0; + double jrx1 = 0; + double jrx2 = 0; + double jry1 = 0; + double jry2 = 0; + double jrz1 = 0; + double jrz2 = 0; + + // Intermediate expressions: + double _lever_p_2 = 0; + double _a2_p_2 = 0; + double __s_a2_p_2 = 0; + double __s_a2_p_2_a_lever_p_2 = 0; + double _1_d_sqrt_l__s_a2_p_2_a_lever_p_2_r_ = 0; + double _lever_p_4 = 0; + double _a1_p_2 = 0; + double __s_a1_p_2_m_a2_p_2 = 0; + double __s_a1_p_2_m_a2_p_2_a_lever_p_4 = 0; + double _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ = 0; + double _lever_p_5 = 0; + double _sin_l_theta0_r_ = 0; + double _lever_p_5_m_sin_l_theta0_r_ = 0; + double _lever_p_8 = 0; + double _lever_p_6 = 0; + double __s_a1_p_2_m_lever_p_6 = 0; + double _a1_p_4 = 0; + double _a2_p_4 = 0; + double __s_a1_p_4_m_a2_p_4 = 0; + double __s_a2_p_2_m_lever_p_6 = 0; + double _sin_l_theta0_r__p_2 = 0; + double __s_2_m_lever_p_8_m_sin_l_theta0_r__p_2 = 0; + double _a1_p_2_m_a2_p_4_m_lever_p_2 = 0; + double _a1_p_4_m_a2_p_2_m_lever_p_2 = 0; + double _2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2 = 0; + double _2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2 = 0; + double _a2_p_3 = 0; + double __s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2 = 0; + double _a1_p_3 = 0; + double __s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2 = 0; + double __s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2 = 0; + double _2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2 = 0; + double _2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2 = 0; + double __s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8 = 0; + double _sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = 0; + double __s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = 0; + double _lever_p_3 = 0; + double __s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r_ = 0; + double _a1_m_a2_p_3_m_lever_m_sin_l_theta0_r_ = 0; + double __s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r_ = 0; + double _a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_ = 0; + double _2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = 0; + double __s_a1_p_2 = 0; + double __s_a1_p_2_a_lever_p_2 = 0; + double _1_d_sqrt_l__s_a1_p_2_a_lever_p_2_r_ = 0; + double __s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = 0; + double __s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r_ = 0; + double _a1_p_3_m_a2_m_lever_m_sin_l_theta0_r_ = 0; + double _a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r_ = 0; + double _2_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = 0; + double _a1_m_a2 = 0; + double _a1_m_a2_a_lever_p_2 = 0; + double _1_d__l_a1_m_a2_a_lever_p_2_r_ = 0; + double __s_lever_p_4 = 0; + double _a1_p_2_m_a2_p_2 = 0; + double _a1_p_2_m_a2_p_2_s_lever_p_4 = 0; + double _cos_l_theta0_r_ = 0; + double _cos_l_theta0_r__p_2 = 0; + double __s_2_m_lever_p_4_m_cos_l_theta0_r__p_2 = 0; + double _a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2 = 0; + double _a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2 = 0; + double __s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4 = 0; + double __l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r_ = 0; + double _a1_m_lever_p_2 = 0; + double _a2_m_lever_p_2 = 0; + double __s_a1_m_a2_p_2 = 0; + double __s_a1_p_2_m_a2 = 0; + double __s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2 = 0; + double __l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2 = 0; + double _lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2 = 0; + double _lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r_ = 0; + double _sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = 0; + double __l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = 0; + double _a1_a_a2 = 0; + double _lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r_ = 0; + double _lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = 0; + double _2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = 0; + double _1_d__l__s_a2_p_2_a_lever_p_2_r_ = 0; + double __l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p__l__s_2_r_ = 0; + double __l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2 = 0; + double __s_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = 0; + double _1_s_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = 0; + double __s_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = 0; + double _1_d_lever = 0; + double _4_m_lever_p_2_m_sin_l_theta0_r__p_2 = 0; + double _1_d__l__s_a1_p_2_a_lever_p_2_r_ = 0; + double __l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2 = 0; + double __s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = 0; + double __s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = 0; + double _4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = 0; + double _sqrt_l_4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r_ = 0; + double _1_d_sin_l_theta0_r_ = 0; + double __s_sqrt_l_4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r__m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__d__l_lever_m_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__r_ = 0; + double __s_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = 0; + double _1_s_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = 0; + double __s_sqrt_l_4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r__m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__d__l_lever_m_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__r_ = 0; + double _sqrt_l_4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r__m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__d__l_lever_m_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__r_ = 0; + double _sqrt_l_4_m_lever_p_2_m_sin_l_theta0_r__p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_m_sin_l_theta0_r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r__m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__d__l_lever_m_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__r_ = 0; + double _lever_p__l__s_2_r_ = 0; + double _2_m_lever_p_2 = 0; + double __s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = 0; + double __s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = 0; + double _2_m_lever_p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = 0; + double __l_2_m_lever_p_2_s_4_m_lever_p_2_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_4_m_lever_p_2_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__p_2_d__l__l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__r__d__l_2_m_lever_p_2_r_ = 0; + double _a2_p_3_m_lever_m_sin_l_theta0_r_ = 0; + double __s_a2_m_lever_p_3_m_sin_l_theta0_r_ = 0; + double _1_d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = 0; + double __s_a1_m_lever_p_6 = 0; + double __s_2_m_a1_p_3_m_a2_p_4 = 0; + double _a1_m_a2_p_4_m_lever_p_2 = 0; + double _a2_m_lever_p_6_m_sin_l_theta0_r__p_2 = 0; + double __s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2 = 0; + double _2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2 = 0; + double _2_m_a1_p_3_m_a2_p_2_m_lever_p_2 = 0; + double __s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2 = 0; + double __s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2 = 0; + double _3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2 = 0; + double __s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2 = 0; + double __s_a2_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = 0; + double _a2_p_3_m_lever_m_sin_l_theta0_r__s_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = 0; + double _2_m_lever_m__l_a2_p_3_m_lever_m_sin_l_theta0_r__s_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = 0; + double _4_m_a1_m_a2_p_2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = 0; + double _4_m_a1_m_a2_p_2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__a_2_m_lever_m__l_a2_p_3_m_lever_m_sin_l_theta0_r__s_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = 0; + double __s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = 0; + double __s_a1_m_lever_p_3_m_sin_l_theta0_r_ = 0; + double __s_a2_m_lever_p_6 = 0; + double __s_2_m_a1_p_4_m_a2_p_3 = 0; + double _a1_m_lever_p_6_m_sin_l_theta0_r__p_2 = 0; + double _a1_p_4_m_a2_m_lever_p_2 = 0; + double __s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2 = 0; + double _2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2 = 0; + double _2_m_a1_p_2_m_a2_p_3_m_lever_p_2 = 0; + double __s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2 = 0; + double __s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2 = 0; + double _3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2 = 0; + double __s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6 = 0; + double __s_a2_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = 0; + double __s_2_m_a2_m_lever_p_3_m_sin_l_theta0_r_ = 0; + double _3_m_a1_m_a2_p_2_m_lever_m_sin_l_theta0_r_ = 0; + double _3_m_a1_m_a2_p_2_m_lever_m_sin_l_theta0_r__s_a1_m_lever_p_3_m_sin_l_theta0_r__s_2_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = 0; + double _2_m_lever_m__l_3_m_a1_m_a2_p_2_m_lever_m_sin_l_theta0_r__s_a1_m_lever_p_3_m_sin_l_theta0_r__s_2_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = 0; + double __l__s_a2_p_2_a_lever_p_2_r__p__l__s_1_t_5_r_ = 0; + double _2_m_a2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l__l__s_a2_p_2_a_lever_p_2_r__p_1_t_5_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = 0; + double _4_m_a1_p_2_m_a2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = 0; + double _4_m_a1_p_2_m_a2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__a_2_m_a2_m_lever_m__l_a1_m_a2_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l__l__s_a2_p_2_a_lever_p_2_r__p_1_t_5_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r__a_2_m_lever_m__l_3_m_a1_m_a2_p_2_m_lever_m_sin_l_theta0_r__s_a1_m_lever_p_3_m_sin_l_theta0_r__s_2_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a2_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a2_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = 0; + double __s_a1_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = 0; + double __s_2_m_a1_m_lever_p_3_m_sin_l_theta0_r_ = 0; + double _3_m_a1_p_2_m_a2_m_lever_m_sin_l_theta0_r_ = 0; + double _3_m_a1_p_2_m_a2_m_lever_m_sin_l_theta0_r__s_2_m_a1_m_lever_p_3_m_sin_l_theta0_r__s_a1_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__s_a2_m_lever_p_3_m_sin_l_theta0_r__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = 0; + double _2_m_lever_m__l_3_m_a1_p_2_m_a2_m_lever_m_sin_l_theta0_r__s_2_m_a1_m_lever_p_3_m_sin_l_theta0_r__s_a1_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__s_a2_m_lever_p_3_m_sin_l_theta0_r__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = 0; + double __l__s_a1_p_2_a_lever_p_2_r__p__l__s_1_t_5_r_ = 0; + double _2_m_a1_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l__l__s_a1_p_2_a_lever_p_2_r__p_1_t_5_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = 0; + double _4_m_a1_m_a2_p_2_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = 0; + double _4_m_a1_m_a2_p_2_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__a_2_m_a1_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l__l__s_a1_p_2_a_lever_p_2_r__p_1_t_5_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r__a_2_m_lever_m__l_3_m_a1_p_2_m_a2_m_lever_m_sin_l_theta0_r__s_2_m_a1_m_lever_p_3_m_sin_l_theta0_r__s_a1_m__l__s_2_m_a1_p_3_m_a2_p_4_a_2_m_a1_p_3_m_a2_p_2_m_lever_p_2_a_3_m_a1_p_2_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_3_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_a2_p_4_m_lever_p_2_s_2_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_m_lever_p_6_s_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__s_a2_m_lever_p_3_m_sin_l_theta0_r__s_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = 0; + double _a1_p_3_m_lever_m_sin_l_theta0_r_ = 0; + double __s_a1_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = 0; + double _a1_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_lever_p_3_m_sin_l_theta0_r__s_a1_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r_ = 0; + double _2_m_lever_m__l_a1_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_lever_p_3_m_sin_l_theta0_r__s_a1_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = 0; + double _4_m_a1_p_2_m_a2_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = 0; + double _4_m_a1_p_2_m_a2_m_lever_m__l_a1_p_3_m_a2_m_lever_m_sin_l_theta0_r__s_a1_p_2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_a2_m_lever_p_3_m_sin_l_theta0_r__s_a1_m_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__a_lever_p_5_m_sin_l_theta0_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__a_2_m_lever_m__l_a1_p_3_m_lever_m_sin_l_theta0_r__s_a1_m_lever_p_3_m_sin_l_theta0_r__s_a1_m__l__s_2_m_a1_p_4_m_a2_p_3_a_a1_p_4_m_a2_m_lever_p_2_a_3_m_a1_p_3_m_a2_p_2_m_lever_p_2_m_sin_l_theta0_r__p_2_s_a1_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_a2_p_3_m_lever_p_2_s_2_m_a1_p_2_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_s_3_m_a1_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_m_lever_p_6_r__d_sqrt_l__s_a1_p_4_m_a2_p_4_a_a1_p_4_m_a2_p_2_m_lever_p_2_a_2_m_a1_p_3_m_a2_p_3_m_lever_p_2_m_sin_l_theta0_r__p_2_s_2_m_a1_p_3_m_a2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_a1_p_2_m_a2_p_4_m_lever_p_2_s_2_m_a1_p_2_m_a2_p_2_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a1_p_2_m_lever_p_6_s_2_m_a1_m_a2_p_3_m_lever_p_4_m_sin_l_theta0_r__p_2_a_2_m_a1_m_a2_m_lever_p_6_m_sin_l_theta0_r__p_2_a_2_m_a2_p_2_m_lever_p_6_m_sin_l_theta0_r__p_2_s_a2_p_2_m_lever_p_6_s_2_m_lever_p_8_m_sin_l_theta0_r__p_2_a_lever_p_8_r__r__m_sin_l_theta0_r__d__l_sqrt_l__s_a1_p_2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = 0; + double _a2_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = 0; + double _lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r_ = 0; + double _1_d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = 0; + double __s_2_m_a1_m_a2_p_2 = 0; + double _2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2 = 0; + double __s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2 = 0; + double __l__s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2 = 0; + double _a1_m_a2_p_2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r_ = 0; + double __s_2_m_a2_p_2 = 0; + double __s_4_m_a1_m_a2 = 0; + double __s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2 = 0; + double _lever_p_2_m__l__s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2 = 0; + double _a1_m_a2_p_2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2 = 0; + double __l_a1_m_a2_a_lever_p_2_r__m__l_a1_m_a2_p_2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = 0; + double __s_2_m_a1_m_a2_p_2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r_ = 0; + double __s_2_m_a1_m_a2_p_2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r__a_a2_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__a_lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m__l_a1_m_a2_p_2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = 0; + double _2_m_lever_m__l__s_2_m_a1_m_a2_p_2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r__a_a2_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__a_lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m__l_a1_m_a2_p_2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = 0; + double __l_a1_m_a2_a_lever_p_2_r__p__l__s_2_r_ = 0; + double __s_2_m_a2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__p_2_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = 0; + double _4_m_a1_m_a2_p_2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = 0; + double _4_m_a1_m_a2_p_2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_2_m_a2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__p_2_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r__a_2_m_lever_m__l__s_2_m_a1_m_a2_p_2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r__a_a2_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__a_lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m__l_a1_m_a2_p_2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_4_m_a1_m_a2_s_2_m_a2_p_2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_m_a2_p_2_a_2_m_a1_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = 0; + double _a1_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = 0; + double __s_2_m_a1_p_2_m_a2 = 0; + double _2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2 = 0; + double __s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2 = 0; + double __l__s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2 = 0; + double _a1_p_2_m_a2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r_ = 0; + double __s_2_m_a1_p_2 = 0; + double __s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2 = 0; + double _lever_p_2_m__l__s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2 = 0; + double _a1_p_2_m_a2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2 = 0; + double __l_a1_m_a2_a_lever_p_2_r__m__l_a1_p_2_m_a2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = 0; + double __s_2_m_a1_p_2_m_a2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r_ = 0; + double __s_2_m_a1_p_2_m_a2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r__a_a1_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__a_lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m__l_a1_p_2_m_a2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r_ = 0; + double _2_m_lever_m__l__s_2_m_a1_p_2_m_a2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r__a_a1_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__a_lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m__l_a1_p_2_m_a2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = 0; + double __s_2_m_a1_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__p_2_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = 0; + double _4_m_a1_p_2_m_a2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r_ = 0; + double _4_m_a1_p_2_m_a2_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__p_2_r__s_2_m_a1_m_lever_m__l_lever_m__l_a1_a_a2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__p_2_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r__a_2_m_lever_m__l__s_2_m_a1_p_2_m_a2_m_lever_m__l_a1_a_a2_r__m_sin_l_theta0_r__a_a1_m_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__a_lever_m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__m_sin_l_theta0_r__a__l_a1_m_a2_a_lever_p_2_r__m__l_a1_p_2_m_a2_m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__a_lever_p_2_m__l__s_2_m_a1_p_2_s_4_m_a1_m_a2_a_2_m_lever_p_2_r__m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__m_sin_l_theta0_r__p_2_d_2_a__l__s_2_m_a1_p_2_m_a2_a_2_m_a2_m_lever_p_2_m_cos_l_theta0_r__p_2_r__m__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__d_2_r__d_sqrt_l_lever_p_2_m__l__s_a1_p_2_m_a2_s_a1_m_a2_p_2_a_a1_m_lever_p_2_a_a2_m_lever_p_2_r__p_2_m_sin_l_theta0_r__p_2_a__l_a1_p_2_m_a2_p_2_s_lever_p_4_r__m__l__s_a1_p_2_m_a2_p_2_a_a1_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_a_a2_p_2_m_lever_p_2_m_cos_l_theta0_r__p_2_s_2_m_lever_p_4_m_cos_l_theta0_r__p_2_a_lever_p_4_r__r__r__m_sin_l_theta0_r__d__l__l_a1_m_a2_a_lever_p_2_r__m__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r__r_ = 0; + +}; + +} + diff --git a/VirtualRobot/Nodes/HemisphereJoint/Joint.cpp b/VirtualRobot/Nodes/HemisphereJoint/Joint.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1d5a26ad82b6099338568961f1008b3cd415cf9d --- /dev/null +++ b/VirtualRobot/Nodes/HemisphereJoint/Joint.cpp @@ -0,0 +1,97 @@ +#include "Joint.h" + +#include <cmath> + +#include <SimoxUtility/math/convert/deg_to_rad.h> +#include <SimoxUtility/math/pose/pose.h> + + +namespace VirtualRobot::hemisphere +{ + + Joint::Joint() : + Joint(1, simox::math::deg_to_rad(25.)) + { + } + + + Joint::Joint(double lever, double theta0) + { + this->setConstants(lever, theta0); + } + + + void Joint::setConstants(double lever, double theta0) + { + this->lever = lever; + this->theta0 = theta0; + this->radius = 2 * std::sin(theta0) * lever; + + this->limitHi = simox::math::deg_to_rad(45 - 6.0); + this->limitLo = - simox::math::deg_to_rad(45 - 14.0); + } + + + void Joint::computeFkOfPosition(double p1, double p2) + { + fk.compute(p1, p2, lever, theta0); + } + + + void Joint::computeFkOfPosition(const Eigen::Vector2d& p12) + { + computeFkOfPosition(p12(0), p12(1)); + } + + + void Joint::computeFkOfAngle(const Eigen::Vector2d& alpha12) + { + computeFkOfPosition(angleToPosition(alpha12)); + } + + + Eigen::Vector3d Joint::getEndEffectorTranslation() const + { + return Eigen::Vector3d { + fk.ex, + fk.ey, + fk.ez + }; + } + + + Eigen::Matrix3d Joint::getEndEffectorRotation() const + { + // r_wrist_to_base = np.array([[exx, eyx, ezx], [exy, eyy, ezy], [exz, eyz, ezz]]) + Eigen::Matrix3d ori; + ori << fk.exx, fk.eyx, fk.ezx, + fk.exy, fk.eyy, fk.ezy, + fk.exz, fk.eyz, fk.ezz; + return ori; + } + + + Eigen::Matrix4d Joint::getEndEffectorTransform() const + { + return simox::math::pose(getEndEffectorTranslation(), getEndEffectorRotation()); + } + + + Joint::Jacobian Joint::getJacobian() const + { + Joint::Jacobian jacobian; + jacobian << fk.jx1, fk.jx2, + fk.jy1, fk.jy2, + fk.jz1, fk.jz2, + fk.jrx1, fk.jrx2, + fk.jry1, fk.jry2, + fk.jrz1, fk.jrz2; + return jacobian; + } + + Eigen::Vector2d Joint::angleToPosition(const Eigen::Vector2d& alpha) const + { + return lever * Eigen::sin((alpha + Eigen::Vector2d::Constant(theta0)).array()); + } + +} diff --git a/VirtualRobot/Nodes/HemisphereJoint/Joint.h b/VirtualRobot/Nodes/HemisphereJoint/Joint.h new file mode 100644 index 0000000000000000000000000000000000000000..7d2bda4861ccf44c13f02d513a3f38378d6eafd1 --- /dev/null +++ b/VirtualRobot/Nodes/HemisphereJoint/Joint.h @@ -0,0 +1,54 @@ +#pragma once + +#include <Eigen/Core> + +#include "Expressions.h" + + +namespace VirtualRobot::hemisphere +{ + + class Joint + { + public: + + using Jacobian = Eigen::Matrix<double, 6, 2>; + + public: + + Joint(); + Joint(double lever, double theta0); + + + void setConstants(double lever, double theta0); + + + void computeFkOfAngle(const Eigen::Vector2d& alpha12); + + void computeFkOfPosition(const Eigen::Vector2d& p12); + void computeFkOfPosition(double p1, double p2); + + + Eigen::Vector3d getEndEffectorTranslation() const; + Eigen::Matrix3d getEndEffectorRotation() const; + Eigen::Matrix4d getEndEffectorTransform() const; + Jacobian getJacobian() const; + + Eigen::Vector2d angleToPosition(const Eigen::Vector2d& alpha) const; + + + public: + + double lever = 0; + double theta0 = 0; + double radius = 0; + + double limitLo = 0; + double limitHi = 0; + + + Expressions fk; + + }; + +} diff --git a/VirtualRobot/Nodes/RobotNode.cpp b/VirtualRobot/Nodes/RobotNode.cpp index faa49eb77f3bec30356ad893915c5107739ec419..97214534294620c2c596c82aef79471dc1070c3d 100644 --- a/VirtualRobot/Nodes/RobotNode.cpp +++ b/VirtualRobot/Nodes/RobotNode.cpp @@ -21,19 +21,19 @@ namespace VirtualRobot { - RobotNode::RobotNode(RobotWeakPtr rob, - const std::string& name, - float jointLimitLo, - float jointLimitHi, - VisualizationNodePtr visualization, - CollisionModelPtr collisionModel, - float jointValueOffset, - const SceneObject::Physics& p, - CollisionCheckerPtr colChecker, - RobotNodeType type) - : GraspableSensorizedObject(name, visualization, collisionModel, p, colChecker) - + RobotNode::RobotNode( + RobotWeakPtr rob, + const std::string& name, + float jointLimitLo, + float jointLimitHi, + VisualizationNodePtr visualization, + CollisionModelPtr collisionModel, + float jointValueOffset, + const SceneObject::Physics& physics, + CollisionCheckerPtr colChecker, + RobotNodeType type) : + GraspableSensorizedObject(name, visualization, collisionModel, physics, colChecker) { nodeType = type; maxVelocity = -1.0f; @@ -116,9 +116,7 @@ namespace VirtualRobot default: VR_ERROR << "RobotNodeType nyi..." << std::endl; - } - } bool RobotNode::getEnforceJointLimits() const @@ -639,6 +637,11 @@ namespace VirtualRobot return false; } + bool RobotNode::isHemisphereJoint() const + { + return false; + } + void RobotNode::setLimitless(bool limitless) { this->limitless = limitless; @@ -862,6 +865,11 @@ namespace VirtualRobot jointLimitHi = hi; } + bool RobotNode::isJoint() const + { + return isRotationalJoint() or isTranslationalJoint() or isHemisphereJoint(); + } + void RobotNode::setMaxTorque(float maxTo) { maxTorque = maxTo; diff --git a/VirtualRobot/Nodes/RobotNode.h b/VirtualRobot/Nodes/RobotNode.h index 113b0d91515c6bb6211e07bd7f0d6a1f982a495d..f0188efac5f6026be1b95aa4bf4a2217554837f8 100644 --- a/VirtualRobot/Nodes/RobotNode.h +++ b/VirtualRobot/Nodes/RobotNode.h @@ -78,15 +78,13 @@ namespace VirtualRobot const std::string& name, float jointLimitLo, float jointLimitHi, - VisualizationNodePtr visualization = VisualizationNodePtr(), - CollisionModelPtr collisionModel = CollisionModelPtr(), + VisualizationNodePtr visualization = nullptr, + CollisionModelPtr collisionModel = nullptr, float jointValueOffset = 0.0f, - const SceneObject::Physics& p = SceneObject::Physics(), - CollisionCheckerPtr colChecker = CollisionCheckerPtr(), + const SceneObject::Physics& physics = {}, + CollisionCheckerPtr colChecker = nullptr, RobotNodeType type = Generic); - /*! - */ ~RobotNode() override; @@ -226,8 +224,10 @@ namespace VirtualRobot */ virtual void setJointLimits(float lo, float hi); + bool isJoint() const; virtual bool isTranslationalJoint() const; virtual bool isRotationalJoint() const; + virtual bool isHemisphereJoint() const; /** * @param limitless wheter this node has joint limits or not. diff --git a/VirtualRobot/Nodes/RobotNodeFactory.h b/VirtualRobot/Nodes/RobotNodeFactory.h index 42b2e7cc2bca419b31ed64f1532ef5f3b3883dae..a180a982c54b5b30722cb83df5f5c099568c8e9f 100644 --- a/VirtualRobot/Nodes/RobotNodeFactory.h +++ b/VirtualRobot/Nodes/RobotNodeFactory.h @@ -34,25 +34,56 @@ namespace VirtualRobot { - class VIRTUAL_ROBOT_IMPORT_EXPORT RobotNodeFactory : public AbstractFactoryMethod<RobotNodeFactory, void*> + class VIRTUAL_ROBOT_IMPORT_EXPORT RobotNodeFactory : + public AbstractFactoryMethod<RobotNodeFactory, void*> { public: RobotNodeFactory() { - ; } virtual ~RobotNodeFactory() { - ; } - virtual RobotNodePtr createRobotNode(RobotPtr /*robot*/, const std::string& /*nodeName*/, VisualizationNodePtr /*visualizationModel*/, CollisionModelPtr /*collisionModel*/, float /*limitLow*/, float /*limitHigh*/, float /*jointValueOffset*/, const Eigen::Matrix4f& /*preJointTransform*/, const Eigen::Vector3f& /*axis*/, const Eigen::Vector3f& /*translationDirection*/, const SceneObject::Physics& /*p*/ = SceneObject::Physics(), RobotNode::RobotNodeType /*rntype*/ = RobotNode::Generic) const + virtual RobotNodePtr createRobotNode( + RobotPtr robot, + const std::string& nodeName, + VisualizationNodePtr visualizationModel, + CollisionModelPtr collisionModel, + float limitLow, + float limitHigh, + float jointValueOffset, + const Eigen::Matrix4f& preJointTransform, + const Eigen::Vector3f& axis, + const Eigen::Vector3f& translationDirection, + const SceneObject::Physics& physics = SceneObject::Physics(), + RobotNode::RobotNodeType rntype = RobotNode::Generic + ) const { - return RobotNodePtr(); + (void) robot, (void) nodeName, (void) visualizationModel, (void) collisionModel; + (void) limitLow, (void) limitHigh, (void) jointValueOffset; + (void) preJointTransform, (void) axis, (void) translationDirection; + (void) physics, (void) rntype; + return nullptr; } - virtual RobotNodePtr createRobotNodeDH(RobotPtr /*robot*/, const std::string& /*nodeName*/, VisualizationNodePtr /*visualizationModel*/, CollisionModelPtr /*collisionModel*/, float /*limitLow*/, float /*limitHigh*/, float /*jointValueOffset*/, const DHParameter& /*dhParameters*/, const SceneObject::Physics& /*p*/ = SceneObject::Physics(), RobotNode::RobotNodeType /*rntype*/ = RobotNode::Generic) const + + virtual RobotNodePtr createRobotNodeDH( + RobotPtr robot, + const std::string& nodeName, + VisualizationNodePtr visualizationModel, + CollisionModelPtr collisionModel, + float limitLow, + float limitHigh, + float jointValueOffset, + const DHParameter& dhParameters, + const SceneObject::Physics& physics = SceneObject::Physics(), + RobotNode::RobotNodeType rntype = RobotNode::Generic) const { - return RobotNodePtr(); + (void) robot, (void) nodeName, (void) visualizationModel, (void) collisionModel; + (void) limitLow, (void) limitHigh, (void) jointValueOffset; + (void) dhParameters; + (void) physics, (void) rntype; + return nullptr; } }; diff --git a/VirtualRobot/Nodes/RobotNodeHemisphere.cpp b/VirtualRobot/Nodes/RobotNodeHemisphere.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ef49ea6008ef9715494400000ab9b31f5b87beeb --- /dev/null +++ b/VirtualRobot/Nodes/RobotNodeHemisphere.cpp @@ -0,0 +1,341 @@ +#include "RobotNodeHemisphere.h" +#include "Nodes/Sensor.h" +#include "Robot.h" +#include "VirtualRobotException.h" + +#include <algorithm> +#include <cmath> + +#include <Eigen/Geometry> + +#include <SimoxUtility/meta/enum/EnumNames.hpp> +#include <SimoxUtility/math/pose/pose.h> + + +namespace VirtualRobot +{ + + static const float limit = 1.0; + + extern const simox::meta::EnumNames<RobotNodeHemisphere::Role> RoleNames = + { + { RobotNodeHemisphere::Role::FIRST, "first" }, + { RobotNodeHemisphere::Role::SECOND, "second" }, + }; + + VirtualRobot::RobotNodeHemisphere::RobotNodeHemisphere() + { + } + + RobotNodeHemisphere::Role RobotNodeHemisphere::RoleFromString(const std::string& string) + { + return RoleNames.from_name(string); + } + + RobotNodeHemisphere::RobotNodeHemisphere( + RobotWeakPtr rob, + const std::string& name, + float jointLimitLo, + float jointLimitHi, + const Eigen::Matrix4f& preJointTransform, + const Eigen::Vector3f& axis, + VisualizationNodePtr visualization, + CollisionModelPtr collisionModel, + float jointValueOffset, + const SceneObject::Physics& physics, + CollisionCheckerPtr colChecker, + RobotNodeType type + ) : + RobotNode(rob, name, -limit, limit, visualization, collisionModel, + jointValueOffset, physics, colChecker, type) + { + (void) axis; + (void) jointLimitLo, (void) jointLimitHi; + + initialized = false; + optionalDHParameter.isSet = false; + localTransformation = preJointTransform; + checkValidRobotNodeType(); + } + + + RobotNodeHemisphere::RobotNodeHemisphere( + RobotWeakPtr rob, + const std::string& name, + float jointLimitLo, + float jointLimitHi, + float a, float d, float alpha, float theta, + VisualizationNodePtr visualization, + CollisionModelPtr collisionModel, + float jointValueOffset, + const SceneObject::Physics& physics, + CollisionCheckerPtr colChecker, + RobotNodeType type + ) : + RobotNode(rob, name, jointLimitLo, jointLimitHi, visualization, collisionModel, + jointValueOffset, physics, colChecker, type) + { + initialized = false; + optionalDHParameter.isSet = true; + optionalDHParameter.setAInMM(a); + optionalDHParameter.setDInMM(d); + optionalDHParameter.setAlphaRadian(alpha, true); + optionalDHParameter.setThetaRadian(theta, true); + + // compute DH transformation matrices + Eigen::Matrix4f RotTheta = Eigen::Matrix4f::Identity(); + RotTheta(0, 0) = cos(theta); + RotTheta(0, 1) = -sin(theta); + RotTheta(1, 0) = sin(theta); + RotTheta(1, 1) = cos(theta); + Eigen::Matrix4f TransD = Eigen::Matrix4f::Identity(); + TransD(2, 3) = d; + Eigen::Matrix4f TransA = Eigen::Matrix4f::Identity(); + TransA(0, 3) = a; + Eigen::Matrix4f RotAlpha = Eigen::Matrix4f::Identity(); + RotAlpha(1, 1) = cos(alpha); + RotAlpha(1, 2) = -sin(alpha); + RotAlpha(2, 1) = sin(alpha); + RotAlpha(2, 2) = cos(alpha); + + localTransformation = RotTheta * TransD * TransA * RotAlpha; + checkValidRobotNodeType(); + } + + + RobotNodeHemisphere::~RobotNodeHemisphere() + = default; + + + void RobotNodeHemisphere::setXmlInfo(const XmlInfo& info) + { + VR_ASSERT(second.has_value()); + switch (info.role) + { + case Role::FIRST: + first.emplace(First{}); + first->math.joint.setConstants(info.lever, info.theta0); + break; + + case Role::SECOND: + second.emplace(Second{}); + break; + } + } + + + bool RobotNodeHemisphere::initialize( + SceneObjectPtr parent, + const std::vector<SceneObjectPtr>& children) + { + VR_ASSERT_MESSAGE(first.has_value() xor second.has_value(), std::stringstream() << first.has_value() << " / " << second.has_value()); + + // The second node needs to store a reference to the first node. + if (second) + { + VR_ASSERT_MESSAGE(not second->first, "Second must not be initialized yet."); + + RobotNodeHemisphere* firstNode = dynamic_cast<RobotNodeHemisphere*>(parent.get()); + RobotNodeHemisphere* secondNode = this; + + if (not (firstNode and firstNode->first)) + { + std::stringstream ss; + ss << "The parent of a hemisphere joint with role '" << RoleNames.to_name(Role::SECOND) << "' " + << "must be a hemisphere joint with role '" << RoleNames.to_name(Role::FIRST) << "' "; + THROW_VR_EXCEPTION(ss.str()); + } + + // Save pointer to firstNode + second->first = firstNode; + + // Set up robot node parameters. + { + const hemisphere::Joint& joint = second->math().joint; + + firstNode->jointLimitLo = joint.limitLo; + secondNode->jointLimitLo = joint.limitLo; + + firstNode->jointLimitHi = joint.limitHi; + secondNode->jointLimitHi = joint.limitHi; + } + } + + return RobotNode::initialize(parent, children); + } + + + void RobotNodeHemisphere::JointMath::update(const Eigen::Vector2f& actuators) + { + if (actuators != this->actuators) + { + joint.computeFkOfAngle(actuators.cast<double>()); + } + } + + + void RobotNodeHemisphere::updateTransformationMatrices( + const Eigen::Matrix4f& parentPose) + { + VR_ASSERT_MESSAGE(first.has_value() xor second.has_value(), std::stringstream() << first.has_value() << " / " << second.has_value()); + + if (first) + { + globalPose = parentPose * localTransformation; + } + else if (second) + { + VR_ASSERT_MESSAGE(second->first, "First node must be known to second node."); + + JointMath& math = second->math(); + Eigen::Vector2f actuators(second->first->getJointValue(), this->getJointValue()); + + math.update(actuators); + + Eigen::Vector3d translation = math.joint.getEndEffectorTranslation(); + const Eigen::Matrix3d rotation = math.joint.getEndEffectorRotation(); + const Eigen::Matrix4d transform = simox::math::pose(translation, rotation); + + // Update Second + { + this->globalPose = parentPose * localTransformation * transform.cast<float>(); + + Eigen::IOFormat iof(5, 0, " ", "\n", " [", "]"); + std::cout << __FUNCTION__ << "() of second actuator with " + << "\n lever = " << math.joint.lever + << "\n theta0 = " << math.joint.theta0 + << "\n radius = " << math.joint.radius + << "\n joint value = " << jointValue + << "\n actuator (angle) = \n" << actuators.transpose().format(iof) + << "\n actuator (pos) = \n" << math.joint.angleToPosition(actuators.cast<double>()).transpose().format(iof) + << "\n local transform = \n" << localTransformation.format(iof) + << "\n joint transform = \n" << transform.format(iof) + << std::endl; + } + } + } + + + void RobotNodeHemisphere::print(bool printChildren, bool printDecoration) const + { + ReadLockPtr lock = getRobot()->getReadLock(); + VR_ASSERT_MESSAGE(first.has_value() xor second.has_value(), std::stringstream() << first.has_value() << " / " << second.has_value()); + + if (printDecoration) + { + std::cout << "******** RobotNodeHemisphere ********" << std::endl; + } + + RobotNode::print(false, false); + + if (first) + { + std::cout << "* Hemisphere joint first node"; + } + else if (second) + { + std::cout << "* Hemisphere joint second node"; + std::cout << "* Transform: \n" << second->math().joint.getEndEffectorTransform() << std::endl; + } + + if (printDecoration) + { + std::cout << "******** End RobotNodeHemisphere ********" << std::endl; + } + + if (printChildren) + { + for (const SceneObjectPtr& child : this->getChildren()) + { + child->print(true, true); + } + } + } + + + RobotNodePtr RobotNodeHemisphere::_clone( + const RobotPtr newRobot, + const VisualizationNodePtr visualizationModel, + const CollisionModelPtr collisionModel, + CollisionCheckerPtr colChecker, + float scaling) + { + ReadLockPtr lock = getRobot()->getReadLock(); + Physics physics = this->physics.scale(scaling); + + RobotNodePtr result; + if (optionalDHParameter.isSet) + { + result.reset(new RobotNodeHemisphere( + newRobot, name, jointLimitLo, jointLimitHi, + optionalDHParameter.aMM() * scaling, + optionalDHParameter.dMM() * scaling, + optionalDHParameter.alphaRadian(), + optionalDHParameter.thetaRadian(), + visualizationModel, collisionModel, + jointValueOffset, + physics, colChecker, nodeType)); + } + else + { + Eigen::Matrix4f localTransform = getLocalTransformation(); + simox::math::position(localTransform) *= scaling; + result.reset(new RobotNodeHemisphere( + newRobot, name, + jointLimitLo, jointLimitHi, + localTransform, Eigen::Vector3f::Zero(), + visualizationModel, collisionModel, + jointValueOffset, physics, colChecker, nodeType)); + } + + return result; + } + + + bool RobotNodeHemisphere::isHemisphereJoint() const + { + return true; + } + + + void RobotNodeHemisphere::checkValidRobotNodeType() + { + RobotNode::checkValidRobotNodeType(); + THROW_VR_EXCEPTION_IF(nodeType == Body || nodeType == Transform, "RobotNodeHemisphere must be a JointNode or a GenericNode"); + } + + + std::string RobotNodeHemisphere::_toXML(const std::string& /*modelPath*/) + { + VR_ASSERT_MESSAGE(first.has_value() xor second.has_value(), std::stringstream() << first.has_value() << " / " << second.has_value()); + + if (first) + { + // TODO + return ""; + } + else + { + JointMath& math = second->math(); + + std::stringstream ss; + ss << "\t\t<Joint type='Hemisphere'>" << std::endl; + ss << "\t\t\t<hemisphere lever='" << math.joint.lever << "' theta0='" << math.joint.theta0 << "' />" << std::endl; + ss << "\t\t\t<limits lo='" << jointLimitLo << "' hi='" << jointLimitHi << "' units='radian'/>" << std::endl; + ss << "\t\t\t<MaxAcceleration value='" << maxAcceleration << "'/>" << std::endl; + ss << "\t\t\t<MaxVelocity value='" << maxVelocity << "'/>" << std::endl; + ss << "\t\t\t<MaxTorque value='" << maxTorque << "'/>" << std::endl; + std::map< std::string, float >::iterator propIt = propagatedJointValues.begin(); + + while (propIt != propagatedJointValues.end()) + { + ss << "\t\t\t<PropagateJointValue name='" << propIt->first << "' factor='" << propIt->second << "'/>" << std::endl; + propIt++; + } + + ss << "\t\t</Joint>" << std::endl; + return ss.str(); + } + } + +} diff --git a/VirtualRobot/Nodes/RobotNodeHemisphere.h b/VirtualRobot/Nodes/RobotNodeHemisphere.h new file mode 100644 index 0000000000000000000000000000000000000000..7a548e60075203990e86d891f6bad4e511e242e8 --- /dev/null +++ b/VirtualRobot/Nodes/RobotNodeHemisphere.h @@ -0,0 +1,189 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package VirtualRobot +* @author Rainer Kartmann +* @copyright 2022 Rainer Kartmann +* GNU Lesser General Public License +*/ +#pragma once + +#include "../VirtualRobot.h" + +#include "RobotNode.h" +#include "HemisphereJoint/Joint.h" + +#include <Eigen/Core> + +#include <string> +#include <vector> +#include <optional> + + +namespace VirtualRobot +{ + + using RobotNodeHemispherePtr = std::shared_ptr<class RobotNodeHemisphere>; + + class VIRTUAL_ROBOT_IMPORT_EXPORT RobotNodeHemisphere : public RobotNode + { + public: + + enum class Role + { + FIRST, + SECOND, + }; + static Role RoleFromString(const std::string& string); + + struct XmlInfo + { + Role role; + + // Only set for first: + double theta0 = -1; + double lever = -1; + }; + + friend class RobotFactory; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + + RobotNodeHemisphere( + RobotWeakPtr rob, ///< The robot + const std::string& name, ///< The name + float jointLimitLo, ///< lower joint limit + float jointLimitHi, ///< upper joint limit + const Eigen::Matrix4f& preJointTransform, ///< This transformation is applied before the translation of the joint is done + const Eigen::Vector3f& axis, ///< The rotation axis (in local joint coord system) + VisualizationNodePtr visualization = nullptr, ///< A visualization model + CollisionModelPtr collisionModel = nullptr, ///< A collision model + float jointValueOffset = 0.0f, ///< An offset that is internally added to the joint value + const SceneObject::Physics& p = {}, ///< physics information + CollisionCheckerPtr colChecker = nullptr, ///< A collision checker instance (if not set, the global col checker is used) + RobotNodeType type = Generic + ); + + RobotNodeHemisphere( + RobotWeakPtr rob, ///< The robot + const std::string& name, ///< The name + float jointLimitLo, ///< lower joint limit + float jointLimitHi, ///< upper joint limit + float a, ///< dh paramters + float d, ///< dh paramters + float alpha, ///< dh paramters + float theta, ///< dh paramters + VisualizationNodePtr visualization = nullptr, ///< A visualization model + CollisionModelPtr collisionModel = nullptr, ///< A collision model + float jointValueOffset = 0.0f, ///< An offset that is internally added to the joint value + const SceneObject::Physics& p = {}, ///< physics information + CollisionCheckerPtr colChecker = {}, ///< A collision checker instance (if not set, the global col checker is used) + RobotNodeType type = Generic + ); + + public: + + ~RobotNodeHemisphere() override; + + + void setXmlInfo(const XmlInfo& info); + + bool + initialize( + SceneObjectPtr parent = nullptr, + const std::vector<SceneObjectPtr>& children = {} + ) override; + + /// Print status information. + void + print( + bool printChildren = false, + bool printDecoration = true + ) const override; + + bool + isHemisphereJoint() const override; + + + protected: + + RobotNodeHemisphere(); + + /// Derived classes add custom XML tags here + std::string + _toXML( + const std::string& modelPath + ) override; + + /// Checks if nodeType constraints are fulfilled. Otherwise an exception is thrown. + /// Called on initialization. + void + checkValidRobotNodeType() override; + + void + updateTransformationMatrices( + const Eigen::Matrix4f& parentPose + ) override; + + RobotNodePtr + _clone( + const RobotPtr newRobot, + const VisualizationNodePtr visualizationModel, + const CollisionModelPtr collisionModel, + CollisionCheckerPtr colChecker, + float scaling + ) override; + + + protected: + + struct JointMath + { + /// The actuator values that were used to compute the joint math. + Eigen::Vector2f actuators = Eigen::Vector2f::Constant(std::numeric_limits<float>::min()); + /// The joint math. + hemisphere::Joint joint; + + void update(const Eigen::Vector2f& actuators); + }; + + struct First + { + JointMath math; + }; + std::optional<First> first; + + struct Second + { + /// The first actuator node. + RobotNodeHemisphere* first = nullptr; + + JointMath& math() + { + return first->first->math; + } + const JointMath& math() const + { + return first->first->math; + } + }; + std::optional<Second> second; + + }; + +} // namespace VirtualRobot + diff --git a/VirtualRobot/Nodes/RobotNodeHemisphereFactory.cpp b/VirtualRobot/Nodes/RobotNodeHemisphereFactory.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0fade1edc79f45ab2d743d9ed1e9fede66b1946d --- /dev/null +++ b/VirtualRobot/Nodes/RobotNodeHemisphereFactory.cpp @@ -0,0 +1,100 @@ +/** +* @package VirtualRobot +* @author Rainer Kartmann +* @copyright 2022 Rainer Kartmann +*/ + +#include "RobotNodeHemisphereFactory.h" +#include "RobotNode.h" +#include "RobotNodeHemisphere.h" +#include "../CollisionDetection/CollisionModel.h" + + +namespace VirtualRobot +{ + + RobotNodeHemisphereFactory::RobotNodeHemisphereFactory() + = default; + + + RobotNodeHemisphereFactory::~RobotNodeHemisphereFactory() + = default; + + + RobotNodePtr RobotNodeHemisphereFactory::createRobotNode( + RobotPtr robot, + const std::string& nodeName, + VisualizationNodePtr visualizationModel, + CollisionModelPtr collisionModel, + float limitLow, + float limitHigh, + float jointValueOffset, + const Eigen::Matrix4f& preJointTransform, + const Eigen::Vector3f& axis, + const Eigen::Vector3f& /*translationDirection*/, + const SceneObject::Physics& physics, + RobotNode::RobotNodeType rntype) const + { + std::cout << "CREATE NEW HEMISPHERE JOINT" << std::endl; + return std::make_shared<RobotNodeHemisphere>( + robot, + nodeName, + limitLow, + limitHigh, + preJointTransform, + axis, + visualizationModel, + collisionModel, + jointValueOffset, + physics, + (collisionModel ? collisionModel->getCollisionChecker() : CollisionCheckerPtr()), + rntype); + } + + + RobotNodePtr RobotNodeHemisphereFactory::createRobotNodeDH( + RobotPtr robot, + const std::string& nodeName, + VisualizationNodePtr visualizationModel, + CollisionModelPtr collisionModel, + float limitLow, + float limitHigh, + float jointValueOffset, + const DHParameter& dhParameters, + const SceneObject::Physics& physics, + RobotNode::RobotNodeType rntype) const + { + std::cout << "CREATE NEW HEMISPHERE JOINT DH" << std::endl; + return std::make_shared<RobotNodeHemisphere>( + robot, + nodeName, + limitLow, + limitHigh, + dhParameters.aMM(), + dhParameters.dMM(), + dhParameters.alphaRadian(), + dhParameters.thetaRadian(), + visualizationModel, + collisionModel, + jointValueOffset, + physics, + CollisionCheckerPtr(), + rntype); + } + + + RobotNodeFactory::SubClassRegistry RobotNodeHemisphereFactory::registry(RobotNodeHemisphereFactory::getName(), &RobotNodeHemisphereFactory::createInstance); + + + std::string RobotNodeHemisphereFactory::getName() + { + return "hemisphere"; + } + + + std::shared_ptr<RobotNodeFactory> RobotNodeHemisphereFactory::createInstance(void*) + { + return std::make_shared<RobotNodeHemisphereFactory>(); + } + +} // namespace VirtualRobot diff --git a/VirtualRobot/Nodes/RobotNodeHemisphereFactory.h b/VirtualRobot/Nodes/RobotNodeHemisphereFactory.h new file mode 100644 index 0000000000000000000000000000000000000000..2433a7f19809438e18cbcea01286a13f0e169287 --- /dev/null +++ b/VirtualRobot/Nodes/RobotNodeHemisphereFactory.h @@ -0,0 +1,88 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package VirtualRobot +* @author Rainer Kartmann +* @copyright 2022 Rainer Kartmann +* GNU Lesser General Public License +*/ +#pragma once + +#include "../VirtualRobot.h" +#include "../SceneObject.h" +#include "RobotNodeFactory.h" + + +namespace VirtualRobot +{ + class RobotNode; + + class VIRTUAL_ROBOT_IMPORT_EXPORT RobotNodeHemisphereFactory : public RobotNodeFactory + { + public: + RobotNodeHemisphereFactory(); + ~RobotNodeHemisphereFactory() override; + + /** + * Create a VirtualRobot::RobotNodeHemisphere. + * + * \return instance of VirtualRobot::RobotNodeHemisphere. + */ + RobotNodePtr createRobotNode( + RobotPtr robot, + const std::string& nodeName, + VisualizationNodePtr visualizationModel, + CollisionModelPtr collisionModel, + float limitLow, + float limitHigh, + float jointValueOffset, + const Eigen::Matrix4f& preJointTransform, + const Eigen::Vector3f& axis, + const Eigen::Vector3f& translationDirection, + const SceneObject::Physics& p = SceneObject::Physics(), + RobotNode::RobotNodeType rntype = RobotNode::Generic + ) const override; + + /** + * Create a VirtualRobot::RobotNodeHemisphere from DH parameters. + * + * \return instance of VirtualRobot::RobotNodeHemisphere. + */ + RobotNodePtr createRobotNodeDH( + RobotPtr robot, + const std::string& nodeName, + VisualizationNodePtr visualizationModel, + CollisionModelPtr collisionModel, + float limitLow, + float limitHigh, + float jointValueOffset, + const DHParameter& dhParameters, + const SceneObject::Physics& p = SceneObject::Physics(), + RobotNode::RobotNodeType rntype = RobotNode::Generic + ) const override; + + // AbstractFactoryMethod + public: + static std::string getName(); + static std::shared_ptr<RobotNodeFactory> createInstance(void*); + + private: + static SubClassRegistry registry; + + }; + +} // namespace VirtualRobot + diff --git a/VirtualRobot/Nodes/RobotNodeRevolute.h b/VirtualRobot/Nodes/RobotNodeRevolute.h index 37f1bfd79f862b4d362c5eae4838f41deca8178b..784b2b7e9bc45e45d7b891434de4e996a535a9c7 100644 --- a/VirtualRobot/Nodes/RobotNodeRevolute.h +++ b/VirtualRobot/Nodes/RobotNodeRevolute.h @@ -43,8 +43,6 @@ namespace VirtualRobot EIGEN_MAKE_ALIGNED_OPERATOR_NEW - /*! - */ RobotNodeRevolute(RobotWeakPtr rob, //!< The robot const std::string& name, //!< The name @@ -72,15 +70,12 @@ namespace VirtualRobot const SceneObject::Physics& p = SceneObject::Physics(), //!< physics information CollisionCheckerPtr colChecker = CollisionCheckerPtr(), //!< A collision checker instance (if not set, the global col checker is used) RobotNodeType type = Generic); - /*! - */ + ~RobotNodeRevolute() override; - bool initialize(SceneObjectPtr parent = SceneObjectPtr(), const std::vector<SceneObjectPtr>& children = std::vector<SceneObjectPtr>()) override; + bool initialize(SceneObjectPtr parent = nullptr, const std::vector<SceneObjectPtr>& children = {}) override; - /*! - Print status information. - */ + //! Print status information. void print(bool printChildren = false, bool printDecoration = true) const override; bool isRotationalJoint() const override; @@ -88,11 +83,9 @@ namespace VirtualRobot Standard: In global coordinate system. \param coordSystem When not set the axis is transformed to global coordinate system. Otherwise any scene object can be used as coord system. */ - Eigen::Vector3f getJointRotationAxis(const SceneObjectPtr coordSystem = SceneObjectPtr()) const; + Eigen::Vector3f getJointRotationAxis(const SceneObjectPtr coordSystem = nullptr) const; - /*! - This is the original joint axis, without any transformations applied. - */ + //! This is the original joint axis, without any transformations applied. Eigen::Vector3f getJointRotationAxisInJointCoordSystem() const; /*! @@ -112,7 +105,15 @@ namespace VirtualRobot virtual float getLMomentArm(float angle); void setJointRotationAxis(Eigen::Vector3f newAxis); + + protected: + + RobotNodeRevolute() {}; + + //! Derived classes add custom XML tags here + std::string _toXML(const std::string& modelPath) override; + /*! Can be called by a RobotNodeActuator in order to set the pose of this node. This is useful, if the node is actuated externally, i.e. via a physics engine. @@ -124,19 +125,18 @@ namespace VirtualRobot //! Checks if nodeType constraints are fulfilled. Otherwise an exception is thrown. Called on initialization. void checkValidRobotNodeType() override; - RobotNodeRevolute() {}; - void updateTransformationMatrices(const Eigen::Matrix4f& parentPose) override; - Eigen::Vector3f jointRotationAxis; // eRevoluteJoint (given in local joint coord system) + RobotNodePtr _clone(const RobotPtr newRobot, const VisualizationNodePtr visualizationModel, + const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker, + float scaling) override; - RobotNodePtr _clone(const RobotPtr newRobot, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker, float scaling) override; - /*! - Derived classes add custom XML tags here - */ - std::string _toXML(const std::string& modelPath) override; + protected: + + Eigen::Vector3f jointRotationAxis; // eRevoluteJoint (given in local joint coord system) Eigen::Matrix4f tmpRotMat; + }; typedef std::shared_ptr<RobotNodeRevolute> RobotNodeRevolutePtr; diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp index ff43128cb24d73daf85bbce79cac3395d5fa17e2..d3fdbf94cb7c156cd527be74f077e973abe6f5a7 100644 --- a/VirtualRobot/Robot.cpp +++ b/VirtualRobot/Robot.cpp @@ -947,7 +947,7 @@ namespace VirtualRobot } } - VirtualRobot::RobotConfigPtr Robot::getConfig() + VirtualRobot::RobotConfigPtr Robot::getConfig() const { RobotConfigPtr r(new RobotConfig(shared_from_this(), getName())); diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h index 24134375b83d7df0a083662d7d5f573ae5a591f9..1f8f2328aab89b40f61c77b9c4922cedf1c9af26 100644 --- a/VirtualRobot/Robot.h +++ b/VirtualRobot/Robot.h @@ -194,7 +194,7 @@ namespace VirtualRobot /*! get the complete setup of all robot nodes */ - virtual RobotConfigPtr getConfig(); + virtual RobotConfigPtr getConfig() const; /*! Sets the configuration according to the RobotNodes, defined in c. All other nodes are not affected. */ diff --git a/VirtualRobot/Visualization/TriMeshUtils.h b/VirtualRobot/Visualization/TriMeshUtils.h index 1c055d1bad07a1733170ee8826afc7746cd87fe8..473e9ae34334598e62aa943eb6be8642fc058ae0 100644 --- a/VirtualRobot/Visualization/TriMeshUtils.h +++ b/VirtualRobot/Visualization/TriMeshUtils.h @@ -23,8 +23,11 @@ */ #pragma once +#include <optional> #include "TriMeshModel.h" +#include <optional> + namespace VirtualRobot { class TriMeshUtils diff --git a/VirtualRobot/XML/RobotIO.cpp b/VirtualRobot/XML/RobotIO.cpp index 9bbb565dd5f967528c2daaf1117bf8bb26224b1d..c78f5a3245b0a0293ab572cc4e7d812fb53c57a7 100644 --- a/VirtualRobot/XML/RobotIO.cpp +++ b/VirtualRobot/XML/RobotIO.cpp @@ -7,6 +7,7 @@ #include "../EndEffector/EndEffector.h" #include "../EndEffector/EndEffectorActor.h" #include "../Nodes/RobotNodeFactory.h" +#include "../Nodes/RobotNodeHemisphere.h" #include "../Nodes/RobotNodeFixedFactory.h" #include "../Nodes/RobotNodePrismatic.h" #include "../Transformation/DHParameter.h" @@ -17,11 +18,13 @@ #include "../RuntimeEnvironment.h" #include "VirtualRobot.h" #include "rapidxml.hpp" -#include <SimoxUtility/xml/rapidxml/rapidxml_print.hpp> #include "mujoco/RobotMjcf.h" #include <VirtualRobot/Import/URDF/SimoxURDFFactory.h> +#include <SimoxUtility/xml/rapidxml/rapidxml_print.hpp> #include <SimoxUtility/filesystem/remove_trailing_separator.h> +#include <SimoxUtility/math/convert/deg_to_rad.h> +#include <SimoxUtility/algorithm/string/string_tools.h> #include <vector> #include <fstream> @@ -194,44 +197,43 @@ namespace VirtualRobot rapidxml::xml_attribute<>* attr; float jointOffset = 0.0f; float initialvalue = 0.0f; - std::string jointType; - - RobotNodePtr robotNode; if (!jointXMLNode) { // no <Joint> tag -> fixed joint + RobotNodePtr robotNode; RobotNodeFactoryPtr fixedNodeFactory = RobotNodeFactory::fromName(RobotNodeFixedFactory::getName(), nullptr); - if (fixedNodeFactory) { - robotNode = fixedNodeFactory->createRobotNode(robot, robotNodeName, visualizationNode, collisionModel, jointLimitLow, jointLimitHigh, jointOffset, preJointTransform, axis, translationDir, physics, rntype); + robotNode = fixedNodeFactory->createRobotNode( + robot, robotNodeName, visualizationNode, collisionModel, + jointLimitLow, jointLimitHigh, jointOffset, + preJointTransform, axis, translationDir, + physics, rntype); } - return robotNode; } + std::string jointType; attr = jointXMLNode->first_attribute("type", 0, false); - if (attr) { jointType = getLowerCase(attr->value()); } else { - VR_WARNING << "No 'type' attribute for <Joint> tag. Assuming fixed joint for RobotNode " << robotNodeName << "!" << std::endl; + VR_WARNING << "No 'type' attribute for <Joint> tag. " + << "Assuming fixed joint for RobotNode " << robotNodeName << "!" << std::endl; jointType = RobotNodeFixedFactory::getName(); } attr = jointXMLNode->first_attribute("offset", 0, false); - if (attr) { jointOffset = convertToFloat(attr->value()); } attr = jointXMLNode->first_attribute("initialvalue", 0, false); - if (attr) { initialvalue = convertToFloat(attr->value()); @@ -248,14 +250,16 @@ namespace VirtualRobot bool scaleVisu = false; Eigen::Vector3f scaleVisuFactor = Eigen::Vector3f::Zero(); + std::optional<RobotNodeHemisphere::XmlInfo> hemisphere; + while (node) { - std::string nodeName = getLowerCase(node->name()); + const std::string nodeName = getLowerCase(node->name()); if (nodeName == "dh") { - THROW_VR_EXCEPTION("DH specification in Joint tag is DEPRECATED! Use <RobotNode><Transform><DH>...</DH></Transform><Joint>...</Joint></RobotNode> structure") - + THROW_VR_EXCEPTION("DH specification in Joint tag is DEPRECATED! " + "Use <RobotNode><Transform><DH>...</DH></Transform><Joint>...</Joint></RobotNode> structure.") //THROW_VR_EXCEPTION_IF(dhXMLNode, "Multiple DH definitions in <Joint> tag of robot node <" << robotNodeName << ">." << endl); //dhXMLNode = node; } @@ -267,7 +271,8 @@ namespace VirtualRobot } else if (nodeName == "prejointtransform") { - THROW_VR_EXCEPTION("PreJointTransform is DEPRECATED! Use <RobotNode><Transform>...</Transform><Joint>...</Joint></RobotNode> structure") + THROW_VR_EXCEPTION("PreJointTransform is DEPRECATED! " + "Use <RobotNode><Transform>...</Transform><Joint>...</Joint></RobotNode> structure") //THROW_VR_EXCEPTION_IF(prejointTransformNode, "Multiple preJoint definitions in <Joint> tag of robot node <" << robotNodeName << ">." << endl); //prejointTransformNode = node; } @@ -283,7 +288,8 @@ namespace VirtualRobot } else if (nodeName == "postjointtransform") { - THROW_VR_EXCEPTION("postjointtransform is DEPRECATED and not longer allowed! Use <RobotNode><Transform>...</Transform><Joint>...</Joint></RobotNode> structure") + THROW_VR_EXCEPTION("postjointtransform is DEPRECATED and not longer allowed! " + "Use <RobotNode><Transform>...</Transform><Joint>...</Joint></RobotNode> structure") //THROW_VR_EXCEPTION_IF(postjointTransformNode, "Multiple postjointtransform definitions in <Joint> tag of robot node <" << robotNodeName << ">." << endl); //postjointTransformNode = node; } @@ -442,6 +448,31 @@ namespace VirtualRobot scaleVisuFactor[1] = getFloatByAttributeName(node, "y"); scaleVisuFactor[2] = getFloatByAttributeName(node, "z"); } + else if (nodeName == "hemisphere") + { + hemisphere.emplace(); + + std::string roleString = processStringAttribute("role", node, true); + roleString = simox::alg::to_lower(roleString); + try + { + hemisphere->role = RobotNodeHemisphere::RoleFromString(roleString); + } + catch (const std::out_of_range& e) + { + THROW_VR_EXCEPTION("Invalid role in hemisphere joint: " << e.what()) + } + + switch (hemisphere->role) + { + case RobotNodeHemisphere::Role::FIRST: + hemisphere->lever = getFloatByAttributeName(node, "lever"); + hemisphere->theta0 = simox::math::deg_to_rad(getFloatByAttributeName(node, "theta0")); + break; + case RobotNodeHemisphere::Role::SECOND: + break; + } + } else { THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in <Joint> tag of RobotNode <" << robotNodeName << ">." << endl); @@ -501,14 +532,16 @@ namespace VirtualRobot if (scaleVisu) { THROW_VR_EXCEPTION_IF(scaleVisuFactor.norm() == 0.0f, "Zero scale factor"); - } } + else if (jointType == "hemisphere") + { + } //} + RobotNodePtr robotNode; RobotNodeFactoryPtr robotNodeFactory = RobotNodeFactory::fromName(jointType, nullptr); - if (robotNodeFactory) { /*if (dh.isSet) @@ -517,7 +550,12 @@ namespace VirtualRobot } else {*/ // create nodes that are not defined via DH parameters - robotNode = robotNodeFactory->createRobotNode(robot, robotNodeName, visualizationNode, collisionModel, jointLimitLow, jointLimitHigh, jointOffset, preJointTransform, axis, translationDir, physics, rntype); + robotNode = robotNodeFactory->createRobotNode( + robot, robotNodeName, visualizationNode, collisionModel, + jointLimitLow, jointLimitHigh, jointOffset, + preJointTransform, axis, translationDir, + physics, rntype + ); //} } else @@ -532,7 +570,7 @@ namespace VirtualRobot robotNode->jointValue = initialvalue; - if (robotNode->isRotationalJoint() || robotNode->isTranslationalJoint()) + if (robotNode->isJoint()) { if (robotNode->jointValue < robotNode->jointLimitLo) { @@ -543,11 +581,15 @@ namespace VirtualRobot robotNode->jointValue = robotNode->jointLimitHi; } } + if (robotNode->isHemisphereJoint() and hemisphere.has_value()) + { + RobotNodeHemispherePtr node = std::dynamic_pointer_cast<RobotNodeHemisphere>(robotNode); + node->setXmlInfo(hemisphere.value()); + } if (scaleVisu) { - std::shared_ptr<RobotNodePrismatic> rnPM = std::dynamic_pointer_cast<RobotNodePrismatic>(robotNode); - + RobotNodePrismaticPtr rnPM = std::dynamic_pointer_cast<RobotNodePrismatic>(robotNode); if (rnPM) { rnPM->setVisuScaleFactor(scaleVisuFactor); diff --git a/VirtualRobot/data/robots/examples/HemisphereJoint/Joint3Hemisphere.xml b/VirtualRobot/data/robots/examples/HemisphereJoint/Joint3Hemisphere.xml new file mode 100644 index 0000000000000000000000000000000000000000..7cdabb74a2d65856611b5a7f909a84ea42811026 --- /dev/null +++ b/VirtualRobot/data/robots/examples/HemisphereJoint/Joint3Hemisphere.xml @@ -0,0 +1,91 @@ +<?xml version="1.0" encoding="UTF-8" ?> + +<Robot Type="Simple3DoFRobotWithHemisphereJoint" StandardName="Simple3DoFRobotWithHemisphereJoint" RootNode="First"> + + <RobotNode name="First"> + <Visualization enable="true"> + <CoordinateAxis enable="true" scaling="1" text="Root"/> + <File type="Inventor">end.iv</File> + </Visualization> + <Child name="Joint1_Revolute"/> + </RobotNode> + + <RobotNode name="Joint1_Revolute"> + <Joint type="revolute"> + <Limits unit="degree" lo="-90" hi="90"/> + <Axis x="1" y="0" z="0"/> + </Joint> + <Visualization enable="true"> + <File type="Inventor">joint_01_base.iv</File> + <UseAsCollisionModel/> + </Visualization> + <Child name="Joint2_Hemisphere_A"/> + </RobotNode> + + <RobotNode name="Joint2_Hemisphere_A"> + <Transform> + <Translation x="0" y="0" z="300" /> + </Transform> + <Joint type="hemisphere"> + <hemisphere role="first" lever="40" theta0="25" /> + </Joint> + <Visualization enable="true"> + <File type="Inventor">joint_02_hemisphere_a.iv</File> + <UseAsCollisionModel/> + </Visualization> + <Child name="Joint2_Hemisphere_B"/> + </RobotNode> + + <RobotNode name="Joint2_Hemisphere_B"> + <Joint type="hemisphere"> + <hemisphere role="second" /> + </Joint> + <Visualization enable="true"> + <File type="Inventor">joint_02_hemisphere_b.iv</File> + <UseAsCollisionModel/> + </Visualization> + <Child name="Joint3_Revolute"/> + </RobotNode> + + <RobotNode name="Joint3_Revolute"> + <Transform> + <Translation x="0" y="0" z="50" /> + </Transform> + <Joint type="revolute"> + <Axis x="1" y="0" z="0"/> + <Limits unit="degree" lo="-90" hi="90"/> + </Joint> + <Visualization enable="true"> + <File type="Inventor">joint_03_finger.iv</File> + <UseAsCollisionModel/> + </Visualization> + <Child name="Last"/> + </RobotNode> + + <RobotNode name="Last"> + <Transform> + <Translation x="0" y="0" z="300" /> + </Transform> + <Visualization enable="true"> + <File type="Inventor">end.iv</File> + <UseAsCollisionModel/> + </Visualization> + </RobotNode> + + <RobotNodeSet name="AllJoints" kinematicRoot="Joint1_Revolute"> + <Node name="Joint1_Revolute"/> + <Node name="Joint2_Hemisphere_A"/> + <Node name="Joint2_Hemisphere_B"/> + <Node name="Joint3_Revolute"/> + </RobotNodeSet> + + <RobotNodeSet name="CollisionModel"> + <Node name="First"/> + <Node name="Joint1_Revolute"/> + <Node name="Joint2_Hemisphere_A"/> + <Node name="Joint2_Hemisphere_B"/> + <Node name="Joint3_Revolute"/> + <Node name="Last"/> + </RobotNodeSet> + +</Robot> diff --git a/VirtualRobot/data/robots/examples/HemisphereJoint/end.iv b/VirtualRobot/data/robots/examples/HemisphereJoint/end.iv new file mode 100644 index 0000000000000000000000000000000000000000..9e531b60c00ba67fd5354a09b4601195a20a908c --- /dev/null +++ b/VirtualRobot/data/robots/examples/HemisphereJoint/end.iv @@ -0,0 +1,16 @@ +#Inventor V2.1 ascii + +Separator { + Units { + units MILLIMETERS + } + Material { + diffuseColor 1 0.8 0.4 + } + Cube + { + width 50 + height 50 + depth 50 + } +} diff --git a/VirtualRobot/data/robots/examples/HemisphereJoint/joint_01_base.iv b/VirtualRobot/data/robots/examples/HemisphereJoint/joint_01_base.iv new file mode 100644 index 0000000000000000000000000000000000000000..c6a9a936f3d197fc11a51646da9bbf086c47f71b --- /dev/null +++ b/VirtualRobot/data/robots/examples/HemisphereJoint/joint_01_base.iv @@ -0,0 +1,40 @@ +#Inventor V2.0 ascii + +Separator { + + Units { + units MILLIMETERS + } + Material { + diffuseColor 1 1 1 + } + Rotation { + rotation 0 0 1 1.57 + } + Cylinder { + radius 10 + height 100 + parts (SIDES | TOP | BOTTOM) + } + Rotation { + # rotation 0 0 1 1.57 + rotation 1 0 0 1.57 + } + Translation { + translation 0 150 0 + } + Cylinder { + radius 5 + height 300 + parts (SIDES | TOP | BOTTOM) + } + Translation { + translation 0 150 0 + } + Cube + { + width 60 + height 1 + depth 60 + } +} diff --git a/VirtualRobot/data/robots/examples/HemisphereJoint/joint_02_hemisphere_a.iv b/VirtualRobot/data/robots/examples/HemisphereJoint/joint_02_hemisphere_a.iv new file mode 100644 index 0000000000000000000000000000000000000000..e65631a950c15eb9051b713c8ab4a42c51f2b034 --- /dev/null +++ b/VirtualRobot/data/robots/examples/HemisphereJoint/joint_02_hemisphere_a.iv @@ -0,0 +1,15 @@ +#Inventor V2.0 ascii + +Separator { + + Units { + units MILLIMETERS + } + Material { + diffuseColor .5 .5 1 + } + Sphere + { + radius 16.90475 + } +} diff --git a/VirtualRobot/data/robots/examples/HemisphereJoint/joint_02_hemisphere_b.iv b/VirtualRobot/data/robots/examples/HemisphereJoint/joint_02_hemisphere_b.iv new file mode 100644 index 0000000000000000000000000000000000000000..e80f74c3c49b075d675cde5b4337dec56de8605e --- /dev/null +++ b/VirtualRobot/data/robots/examples/HemisphereJoint/joint_02_hemisphere_b.iv @@ -0,0 +1,39 @@ +#Inventor V2.0 ascii + +Separator +{ + + Units + { + units MILLIMETERS + } + Material { + diffuseColor .5 1 .5 + } + + Rotation + { + rotation 1 0 0 1.57 + } + Sphere + { + radius 16.90475 + } + Cube + { + width 60 + height 1 + depth 60 + } + + Translation + { + translation 0 25 0 + } + Cylinder { + radius 5 + height 50 + parts (SIDES | TOP | BOTTOM) + } + +} diff --git a/VirtualRobot/data/robots/examples/HemisphereJoint/joint_03_finger.iv b/VirtualRobot/data/robots/examples/HemisphereJoint/joint_03_finger.iv new file mode 100644 index 0000000000000000000000000000000000000000..6fd94b676846d8ab1d47bf9034e47e174bd130b0 --- /dev/null +++ b/VirtualRobot/data/robots/examples/HemisphereJoint/joint_03_finger.iv @@ -0,0 +1,34 @@ +#Inventor V2.0 ascii + +Separator { + + Units { + units MILLIMETERS + } + Material { + diffuseColor 1 1 1 + } + Rotation { + rotation 0 0 1 1.57 + } + Cylinder { + radius 10 + height 50 + parts (SIDES | TOP | BOTTOM) + } + Rotation { + # rotation 0 0 1 1.57 + rotation 1 0 0 1.57 + } + Translation { + translation 0 150 0 + } + Cylinder { + radius 10 + height 300 + parts (SIDES | TOP | BOTTOM) + } + Translation { + translation 0 150 0 + } +} diff --git a/VirtualRobot/examples/CMakeLists.txt b/VirtualRobot/examples/CMakeLists.txt index af4bbe78678e576083f97dd316b9e22738a2f5e0..2a5d34cf50c5a7aec301c08e7aa82c5e516b2f3d 100644 --- a/VirtualRobot/examples/CMakeLists.txt +++ b/VirtualRobot/examples/CMakeLists.txt @@ -30,4 +30,5 @@ ADD_SUBDIRECTORY(RGBOffscreenRendering) ADD_SUBDIRECTORY(DepthOffscreenRendering) ADD_SUBDIRECTORY(RobotFromObjects) ADD_SUBDIRECTORY(Iv2Wrl) +ADD_SUBDIRECTORY(HemisphereJoint) ADD_SUBDIRECTORY(TestObjLoading) diff --git a/VirtualRobot/examples/HemisphereJoint/CMakeLists.txt b/VirtualRobot/examples/HemisphereJoint/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..8ec94312fe183d6639f29c0c521b38f85bd47813 --- /dev/null +++ b/VirtualRobot/examples/HemisphereJoint/CMakeLists.txt @@ -0,0 +1,39 @@ +project(HemisphereJoint) + +find_package(Boost ${Simox_BOOST_VERSION} EXACT COMPONENTS system REQUIRED) + +if (Boost_FOUND) + set(SOURCES + main.cpp + ) + set(HEADERS + ) + + set(LIBS + VirtualRobot + ${BOOST_FILESYSTEM_LIBRARIES} + ${BOOST_SYSTEM_LIBRARIES} + ) + + add_executable(${PROJECT_NAME} ${SOURCES} ${HEADERS}) + set_target_properties(${PROJECT_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${Simox_BIN_DIR}) + set_target_properties(${PROJECT_NAME} PROPERTIES FOLDER "Examples") + + # against undefined reference to boost::filesystem::detail::copy_file + # source: https://stackoverflow.com/a/3500721 + target_compile_definitions(${PROJECT_NAME} PRIVATE -DBOOST_NO_CXX11_SCOPED_ENUMS) + + target_link_libraries(${PROJECT_NAME} PRIVATE ${LIBS}) + + ####################################################################################### + ############################ Setup for installation ################################### + ####################################################################################### + install(TARGETS ${PROJECT_NAME} + # IMPORTANT: Add the library to the "export-set" + EXPORT SimoxTargets + RUNTIME DESTINATION bin COMPONENT bin + COMPONENT dev) + + message( STATUS " ** Simox application ${PROJECT_NAME} will be placed into " ${Simox_BIN_DIR}) + message( STATUS " ** Simox application ${PROJECT_NAME} will be installed into " bin) +endif() diff --git a/VirtualRobot/examples/HemisphereJoint/main.cpp b/VirtualRobot/examples/HemisphereJoint/main.cpp new file mode 100644 index 0000000000000000000000000000000000000000..46693a0c20edf8a282074eb70026189f9b75676d --- /dev/null +++ b/VirtualRobot/examples/HemisphereJoint/main.cpp @@ -0,0 +1,122 @@ +#include <chrono> +#include <iostream> + +#include <SimoxUtility/math/convert/deg_to_rad.h> +#include <SimoxUtility/math/rescale.h> +#include <SimoxUtility/math/statistics/measures.h> + +#include <VirtualRobot/RuntimeEnvironment.h> +#include <VirtualRobot/Nodes/HemisphereJoint/Joint.h> + + +using VirtualRobot::RuntimeEnvironment; + + +/** + * + */ +int main(int argc, char* argv[]) +{ + const Eigen::IOFormat iof(5, 0, " ", "\n", " [", "]", "", ""); + + RuntimeEnvironment::setCaption("Convert .iv to .wrl files"); + + RuntimeEnvironment::considerFlag( + "verbose", "Enable verbose output."); + RuntimeEnvironment::considerFlag( + "debug", "Branch to debug code."); + + RuntimeEnvironment::processCommandLine(argc, argv); + + if (RuntimeEnvironment::hasHelpFlag() + // || !RuntimeEnvironment::hasValue("input") + //|| !RuntimeEnvironment::hasValue("output")) + ) + { + RuntimeEnvironment::printOptions(); + return 0; + } + + const bool verbose = RuntimeEnvironment::hasFlag("verbose"); + const bool debug = RuntimeEnvironment::hasFlag("debug"); + + const double lever = 1, theta0 = simox::math::deg_to_rad(25.); + std::cout << "(lever, theta0) = (" << lever << ", " << theta0 << ") " << std::endl; + + if (not debug) + { + std::vector<double> a1s, a2s; + int num = 100; + double aMin = -0.7, aMax=0.7; + for (int i = 0; i < num; ++i) + { + double value = simox::math::rescale(double(i), double(0), double(num), aMin, aMax); + a1s.push_back(value); + a2s.push_back(value); + } + + + using time_point = std::chrono::system_clock::time_point; + + std::vector<double> durationsUs; + durationsUs.reserve(a1s.size() * a2s.size()); + + for (double a1 : a1s) + { + for (double a2 : a2s) + { + const time_point start = std::chrono::system_clock::now(); + + VirtualRobot::hemisphere::Joint joint; + joint.computeFkOfPosition(a1, a2); + + const Eigen::Vector3d pos = joint.getEndEffectorTranslation(); + const Eigen::Matrix3d ori = joint.getEndEffectorRotation(); + const Eigen::Matrix<double, 6, 2> jacobian = joint.getJacobian(); + + const time_point end = std::chrono::system_clock::now(); + using duration = std::chrono::nanoseconds; + durationsUs.push_back(std::chrono::duration_cast<duration>(end - start).count() / 1000.f); + + if (verbose) + { + std::cout << "(a1, a2) = (" << a1 << ", " << a2 << ")" + << "\n ->" + << "\n pos = \n" << pos.transpose().format(iof) + << "\n ori = \n" << ori.format(iof) + << "\n jac = \n" << jacobian.format(iof) + << std::endl; + } + } + } + + double mean = simox::math::mean(durationsUs); + double stddev = simox::math::stddev(durationsUs, mean); + double min = simox::math::min(durationsUs); + double max = simox::math::max(durationsUs); + + const std::string unit = " us"; + std::cout << "Durations:" + << " " << mean << " +- " << stddev << unit + << ", range: [" << min << unit << " to " << max << unit <<"]" + << std::endl; + } + else + { + double offset = std::asin(theta0); + + double a1 = 0, a2 = 0; + a1 += offset; + a2 += offset; + + VirtualRobot::hemisphere::Joint joint; + joint.computeFkOfPosition(a1, a2); + + const Eigen::Vector3d pos = joint.getEndEffectorTranslation(); + std::cout << "\n pos = \n" << pos.transpose().format(iof) + << std::endl; + + } + + return 0; +} diff --git a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp index a9a0f5e77bf0137fd2c9569da79268d83ed347d2..b3b714c236a9766fff67f0e50f1916800261a449 100644 --- a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp +++ b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp @@ -498,7 +498,7 @@ void showRobotWindow::selectRNS(int nr) { nr--; - if (nr >= (int)robotNodeSets.size()) + if (nr >= static_cast<int>(robotNodeSets.size())) { return; } @@ -547,12 +547,12 @@ void showRobotWindow::selectJoint(int nr) UI.labelMinPos->setText(qMin); UI.labelMaxPos->setText(qMax); float j = currentRobotNode->getJointValue(); - UI.lcdNumberJointValue->display((double)j); + UI.lcdNumberJointValue->display(static_cast<double>(j)); - if (fabs(ma - mi) > 0 && (currentRobotNode->isTranslationalJoint() || currentRobotNode->isRotationalJoint())) + if (std::fabs(ma - mi) > 0 && (currentRobotNode->isJoint())) { UI.horizontalSliderPos->setEnabled(true); - int pos = (int)((j - mi) / (ma - mi) * 1000.0f); + int pos = static_cast<int>((j - mi) / (ma - mi) * 1000.0f); UI.horizontalSliderPos->setValue(pos); } else @@ -585,14 +585,17 @@ void showRobotWindow::jointValueChanged(int pos) { int nr = UI.comboBoxJoint->currentIndex(); - if (nr < 0 || nr >= (int)currentRobotNodes.size()) + if (nr < 0 || nr >= static_cast<int>(currentRobotNodes.size())) { return; } - float fPos = currentRobotNodes[nr]->getJointLimitLo() + (float)pos / 1000.0f * (currentRobotNodes[nr]->getJointLimitHi() - currentRobotNodes[nr]->getJointLimitLo()); + float fPos = currentRobotNodes[nr]->getJointLimitLo() + + static_cast<float>(pos) / 1000.0f + * (currentRobotNodes[nr]->getJointLimitHi() + - currentRobotNodes[nr]->getJointLimitLo()); robot->setJointValue(currentRobotNodes[nr], fPos); - UI.lcdNumberJointValue->display((double)fPos); + UI.lcdNumberJointValue->display(static_cast<double>(fPos)); DiffIKWidget::update(robot); updatePointDistanceVisu(); @@ -603,7 +606,7 @@ void showRobotWindow::showCoordSystem() float size = 0.75f; int nr = UI.comboBoxJoint->currentIndex(); - if (nr < 0 || nr >= (int)currentRobotNodes.size()) + if (nr < 0 || nr >= static_cast<int>(currentRobotNodes.size())) { return; } @@ -678,8 +681,8 @@ void showRobotWindow::testPerformance(RobotPtr robot, RobotNodeSetPtr rns) rns->setJointValues(v); } clock_t end = clock(); - float timeMS = (float)(end - start) / (float)CLOCKS_PER_SEC * 1000.0f; - VR_INFO << "Time (visu on, thread on): " << timeMS / (float)loops << std::endl; + float timeMS = static_cast<float>(end - start) / static_cast<float>(CLOCKS_PER_SEC) * 1000.0f; + VR_INFO << "Time (visu on, thread on): " << timeMS / static_cast<float>(loops) << std::endl; start = clock(); robot->setupVisualization(false, false); @@ -699,8 +702,8 @@ void showRobotWindow::testPerformance(RobotPtr robot, RobotNodeSetPtr rns) rns->setJointValues(v); } end = clock(); - timeMS = (float)(end - start) / (float)CLOCKS_PER_SEC * 1000.0f; - VR_INFO << "Time (visu off, thread on): " << timeMS / (float)loops << std::endl; + timeMS = static_cast<float>(end - start) / static_cast<float>CLOCKS_PER_SEC * 1000.0f; + VR_INFO << "Time (visu off, thread on): " << timeMS / static_cast<float>(loops) << std::endl; start = clock(); robot->setupVisualization(true, false); @@ -717,8 +720,8 @@ void showRobotWindow::testPerformance(RobotPtr robot, RobotNodeSetPtr rns) rns->setJointValues(v); } end = clock(); - timeMS = (float)(end - start) / (float)CLOCKS_PER_SEC * 1000.0f; - VR_INFO << "Time (visu on, thread off): " << timeMS / (float)loops << std::endl; + timeMS = static_cast<float>(end - start) / static_cast<float>CLOCKS_PER_SEC * 1000.0f; + VR_INFO << "Time (visu on, thread off): " << timeMS / static_cast<float>(loops) << std::endl; start = clock(); @@ -736,9 +739,8 @@ void showRobotWindow::testPerformance(RobotPtr robot, RobotNodeSetPtr rns) rns->setJointValues(v); } end = clock(); - timeMS = (float)(end - start) / (float)CLOCKS_PER_SEC * 1000.0f; - VR_INFO << "Time (visu off, thread off): " << timeMS / (float)loops << std::endl; - + timeMS = static_cast<float>(end - start) / static_cast<float>CLOCKS_PER_SEC * 1000.0f; + VR_INFO << "Time (visu off, thread off): " << timeMS / static_cast<float>(loops) << std::endl; } void showRobotWindow::loadRobot() @@ -884,7 +886,7 @@ void showRobotWindow::selectEEF(int nr) UI.comboBoxEndEffectorPS->clear(); currentEEF.reset(); - if (nr < 0 || nr >= (int)eefs.size()) + if (nr < 0 || nr >= static_cast<int>(eefs.size())) { return; } diff --git a/VirtualRobot/tests/VirtualRobotMathToolsTest.cpp b/VirtualRobot/tests/VirtualRobotMathToolsTest.cpp index f2766dc2e63d9fca7cfa88a6f9e586ca3d00ecb1..487aeb606a721aef0cedd37116d4a3c4cea037fe 100644 --- a/VirtualRobot/tests/VirtualRobotMathToolsTest.cpp +++ b/VirtualRobot/tests/VirtualRobotMathToolsTest.cpp @@ -14,7 +14,9 @@ #include <string> +#ifndef M_PIf #define M_PIf (static_cast<float>(M_PI)) +#endif BOOST_AUTO_TEST_SUITE(MathTools) diff --git a/build/.gitkeep b/build/.gitkeep deleted file mode 100644 index 65df063943986e0bdf4c4ecf5e822d953a402fd7..0000000000000000000000000000000000000000 --- a/build/.gitkeep +++ /dev/null @@ -1,4 +0,0 @@ -Git can only track files and not directory. - -Therefore this file is added to all empty directories -which need to be available after a Git clone. diff --git a/python/convert_meshes/pyproject.toml b/python/convert_meshes/pyproject.toml index 19d157fe1d9bca6498a6d4ad7b0a8cfe8ceaf860..ed3da4d2cd79d7a332f28030f349c6d4590007be 100644 --- a/python/convert_meshes/pyproject.toml +++ b/python/convert_meshes/pyproject.toml @@ -6,7 +6,7 @@ authors = ["Rainer Kartmann <rainer dot kartmann at kit dot edu>"] [tool.poetry.dependencies] python = "^3.6.2" -ipython = "5.5" +ipython = "*" pymeshlab = "^2021.10" click = "^8.0.4" rich = "^11.2.0" diff --git a/python/hemisphere-joint-demo/CMakeLists.txt b/python/hemisphere-joint-demo/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/python/hemisphere-joint-demo/README.rst b/python/hemisphere-joint-demo/README.rst new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/python/hemisphere-joint-demo/hemisphere_joint_demo/__init__.py b/python/hemisphere-joint-demo/hemisphere_joint_demo/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/python/hemisphere-joint-demo/hemisphere_joint_demo/app/generate_cpp_expressions.py b/python/hemisphere-joint-demo/hemisphere_joint_demo/app/generate_cpp_expressions.py new file mode 100644 index 0000000000000000000000000000000000000000..a779df75059069ff62203f72974631862a3b9290 --- /dev/null +++ b/python/hemisphere-joint-demo/hemisphere_joint_demo/app/generate_cpp_expressions.py @@ -0,0 +1,219 @@ +import os +import os.path + +from sympy import symbols, sin, cos, sqrt, asin + +from hemisphere_joint_demo.sympy_to_code import SympyToCpp + +# Actuation (P1_z, P2_z) +a1, a2 = symbols('a1 a2') + +# Constants defining geometry. +lever, theta0 = symbols('lever theta0') + + +def fk_pos(): + """ + # EEF position, for 90° + (PE_x, PE_y, PE_z) + From https://colab.research.google.com/drive/11faUc8pS1yWxFrnmt05VqpDsqOwEi_dg#scrollTo=za3fZw9Rq5d9&line=1&uniqifier=1 + """ + ex = 2 * lever * (lever ** 5 * sin(theta0) - lever ** 3 * a1 * a2 * sin(theta0) - lever ** 3 * a2 ** 2 * sin(theta0) + lever * a1 * a2 ** 3 * sin(theta0) - a2 * sqrt(-2 * lever ** 8 * sin(theta0) ** 2 + lever ** 8 + 2 * lever ** 6 * a1 ** 2 * sin(theta0) ** 2 - lever ** 6 * a1 ** 2 + 2 * lever ** 6 * a1 * a2 * sin(theta0) ** 2 + 2 * lever ** 6 * a2 ** 2 * sin(theta0) ** 2 - lever ** 6 * a2 ** 2 - 2 * lever ** 4 * a1 ** 3 * a2 * sin(theta0) ** 2 - 2 * lever ** 4 * a1 ** 2 * a2 ** 2 * sin(theta0) ** 2 - 2 * lever ** 4 * a1 * a2 ** 3 * sin(theta0) ** 2 + lever ** 2 * a1 ** 4 * a2 ** 2 + 2 * lever ** 2 * a1 ** 3 * a2 ** 3 * sin(theta0) ** 2 + lever ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin(theta0) / (sqrt(lever ** 2 - a2 ** 2) * (lever ** 4 - a1 ** 2 * a2 ** 2)) + ey = 2 * lever * (lever ** 5 * sin(theta0) - lever ** 3 * a1 ** 2 * sin(theta0) - lever ** 3 * a1 * a2 * sin(theta0) + lever * a1 ** 3 * a2 * sin(theta0) - a1 * sqrt(-2 * lever ** 8 * sin(theta0) ** 2 + lever ** 8 + 2 * lever ** 6 * a1 ** 2 * sin(theta0) ** 2 - lever ** 6 * a1 ** 2 + 2 * lever ** 6 * a1 * a2 * sin(theta0) ** 2 + 2 * lever ** 6 * a2 ** 2 * sin(theta0) ** 2 - lever ** 6 * a2 ** 2 - 2 * lever ** 4 * a1 ** 3 * a2 * sin(theta0) ** 2 - 2 * lever ** 4 * a1 ** 2 * a2 ** 2 * sin(theta0) ** 2 - 2 * lever ** 4 * a1 * a2 ** 3 * sin(theta0) ** 2 + lever ** 2 * a1 ** 4 * a2 ** 2 + 2 * lever ** 2 * a1 ** 3 * a2 ** 3 * sin(theta0) ** 2 + lever ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin(theta0) / (sqrt(lever ** 2 - a1 ** 2) * (lever ** 4 - a1 ** 2 * a2 ** 2)) + ez = ( + 2 + * lever + * ( + lever + * ( + lever ** 4 + - a1 ** 2 * a2 ** 2 + ) + * (a1 + a2) + * sin(theta0) + + ( # _a1_m_a2_a_lever_p_2 + lever ** 2 # _lever_p_2 + + a1 * a2 # _a1_m_a2 + ) + * sqrt( + lever ** 2 + * ( + lever ** 2 * a1 + + lever ** 2 * a2 + - a1 ** 2 * a2 + - a1 * a2 ** 2 + ) ** 2 + * sin(theta0) ** 2 + + (-lever ** 4 + a1 ** 2 * a2 ** 2) + * ( + -2 + * lever ** 4 + * cos(theta0) ** 2 + + lever ** 4 + + lever ** 2 + * a1 ** 2 + * cos(theta0) ** 2 + + lever ** 2 + * a2 ** 2 + * cos(theta0) ** 2 + - a1 ** 2 + * a2 ** 2 + ) + ) + ) + * sin(theta0) # _sin_l_theta0_r_ + / ( + ( # _1_d__l_a1_m_a2_a_lever_p_2_r_ + lever ** 2 + + a1 * a2 + ) + * ( # _1_d__l__s_a1_p_2_m_a2_p_2_a_lever_p_4_r_ + lever ** 4 + - a1 ** 2 + * a2 ** 2 + ) + ) + ) + return dict(ex=ex, ey=ey, ez=ez,) + + +def fk_ori(pos): + """ + EEF orientation + From https://colab.research.google.com/drive/11faUc8pS1yWxFrnmt05VqpDsqOwEi_dg#scrollTo=aNSv-3Ftv3ps&line=2&uniqifier=1 + """ + ex, ey, ez = pos["ex"], pos["ey"], pos["ez"] + + exx = 1 - ex ** 2 / (2 * lever ** 2 * sin(theta0) ** 2) + exy = -ex * ey / (2 * lever ** 2 * sin(theta0) ** 2) + exz = -ex * sqrt(4 * lever ** 2 * sin(theta0) ** 2 - ex ** 2 - ey ** 2) / (2 * lever ** 2 * sin(theta0) ** 2) + + eyx = -ex * ey / (2 * lever ** 2 * sin(theta0) ** 2) + eyy = 1 - ey ** 2 / (2 * lever ** 2 * sin(theta0) ** 2) + eyz = -ey * sqrt(4 * lever ** 2 * sin(theta0) ** 2 - ex ** 2 - ey ** 2) / (2 * lever ** 2 * sin(theta0) ** 2) + + ezx = ex * sqrt(4 * lever ** 2 * sin(theta0) ** 2 - ex ** 2 - ey ** 2) / (2 * lever ** 2 * sin(theta0) ** 2) + ezy = ey * sqrt(4 * lever ** 2 * sin(theta0) ** 2 - ex ** 2 - ey ** 2) / (2 * lever ** 2 * sin(theta0) ** 2) + ezz = (2 * lever ** 2 - ex ** 2 / sin(theta0) ** 2 - ey ** 2 / sin(theta0) ** 2) / (2 * lever ** 2) + + return dict( + exx=exx, exy=exy, exz=exz, + eyx=eyx, eyy=eyy, eyz=eyz, + ezx=ezx, ezy=ezy, ezz=ezz, + ) + + +def jacobian_pos(): + """ + Jacobian + From https://colab.research.google.com/drive/11faUc8pS1yWxFrnmt05VqpDsqOwEi_dg#scrollTo=xx7_60I1sV9j&line=1&uniqifier=1 + """ + jx1 = 4*lever*a1*a2**2*(lever**5*sin(theta0) - lever**3*a1*a2*sin(theta0) - lever**3*a2**2*sin(theta0) + lever*a1*a2**3*sin(theta0) - a2*sqrt(-2*lever**8*sin(theta0)**2 + lever**8 + 2*lever**6*a1**2*sin(theta0)**2 - lever**6*a1**2 + 2*lever**6*a1*a2*sin(theta0)**2 + 2*lever**6*a2**2*sin(theta0)**2 - lever**6*a2**2 - 2*lever**4*a1**3*a2*sin(theta0)**2 - 2*lever**4*a1**2*a2**2*sin(theta0)**2 - 2*lever**4*a1*a2**3*sin(theta0)**2 + lever**2*a1**4*a2**2 + 2*lever**2*a1**3*a2**3*sin(theta0)**2 + lever**2*a1**2*a2**4 - a1**4*a2**4))*sin(theta0)/(sqrt(lever**2 - a2**2)*(lever**4 - a1**2*a2**2)**2) + 2*lever*(-lever**3*a2*sin(theta0) + lever*a2**3*sin(theta0) - a2*(2*lever**6*a1*sin(theta0)**2 - lever**6*a1 + lever**6*a2*sin(theta0)**2 - 3*lever**4*a1**2*a2*sin(theta0)**2 - 2*lever**4*a1*a2**2*sin(theta0)**2 - lever**4*a2**3*sin(theta0)**2 + 2*lever**2*a1**3*a2**2 + 3*lever**2*a1**2*a2**3*sin(theta0)**2 + lever**2*a1*a2**4 - 2*a1**3*a2**4)/sqrt(-2*lever**8*sin(theta0)**2 + lever**8 + 2*lever**6*a1**2*sin(theta0)**2 - lever**6*a1**2 + 2*lever**6*a1*a2*sin(theta0)**2 + 2*lever**6*a2**2*sin(theta0)**2 - lever**6*a2**2 - 2*lever**4*a1**3*a2*sin(theta0)**2 - 2*lever**4*a1**2*a2**2*sin(theta0)**2 - 2*lever**4*a1*a2**3*sin(theta0)**2 + lever**2*a1**4*a2**2 + 2*lever**2*a1**3*a2**3*sin(theta0)**2 + lever**2*a1**2*a2**4 - a1**4*a2**4))*sin(theta0)/(sqrt(lever**2 - a2**2)*(lever**4 - a1**2*a2**2)) + jx2 = 4*lever*a1**2*a2*(lever**5*sin(theta0) - lever**3*a1*a2*sin(theta0) - lever**3*a2**2*sin(theta0) + lever*a1*a2**3*sin(theta0) - a2*sqrt(-2*lever**8*sin(theta0)**2 + lever**8 + 2*lever**6*a1**2*sin(theta0)**2 - lever**6*a1**2 + 2*lever**6*a1*a2*sin(theta0)**2 + 2*lever**6*a2**2*sin(theta0)**2 - lever**6*a2**2 - 2*lever**4*a1**3*a2*sin(theta0)**2 - 2*lever**4*a1**2*a2**2*sin(theta0)**2 - 2*lever**4*a1*a2**3*sin(theta0)**2 + lever**2*a1**4*a2**2 + 2*lever**2*a1**3*a2**3*sin(theta0)**2 + lever**2*a1**2*a2**4 - a1**4*a2**4))*sin(theta0)/(sqrt(lever**2 - a2**2)*(lever**4 - a1**2*a2**2)**2) + 2*lever*a2*(lever**5*sin(theta0) - lever**3*a1*a2*sin(theta0) - lever**3*a2**2*sin(theta0) + lever*a1*a2**3*sin(theta0) - a2*sqrt(-2*lever**8*sin(theta0)**2 + lever**8 + 2*lever**6*a1**2*sin(theta0)**2 - lever**6*a1**2 + 2*lever**6*a1*a2*sin(theta0)**2 + 2*lever**6*a2**2*sin(theta0)**2 - lever**6*a2**2 - 2*lever**4*a1**3*a2*sin(theta0)**2 - 2*lever**4*a1**2*a2**2*sin(theta0)**2 - 2*lever**4*a1*a2**3*sin(theta0)**2 + lever**2*a1**4*a2**2 + 2*lever**2*a1**3*a2**3*sin(theta0)**2 + lever**2*a1**2*a2**4 - a1**4*a2**4))*sin(theta0)/((lever**2 - a2**2)**(3/2)*(lever**4 - a1**2*a2**2)) + 2*lever*(-lever**3*a1*sin(theta0) - 2*lever**3*a2*sin(theta0) + 3*lever*a1*a2**2*sin(theta0) - a2*(lever**6*a1*sin(theta0)**2 + 2*lever**6*a2*sin(theta0)**2 - lever**6*a2 - lever**4*a1**3*sin(theta0)**2 - 2*lever**4*a1**2*a2*sin(theta0)**2 - 3*lever**4*a1*a2**2*sin(theta0)**2 + lever**2*a1**4*a2 + 3*lever**2*a1**3*a2**2*sin(theta0)**2 + 2*lever**2*a1**2*a2**3 - 2*a1**4*a2**3)/sqrt(-2*lever**8*sin(theta0)**2 + lever**8 + 2*lever**6*a1**2*sin(theta0)**2 - lever**6*a1**2 + 2*lever**6*a1*a2*sin(theta0)**2 + 2*lever**6*a2**2*sin(theta0)**2 - lever**6*a2**2 - 2*lever**4*a1**3*a2*sin(theta0)**2 - 2*lever**4*a1**2*a2**2*sin(theta0)**2 - 2*lever**4*a1*a2**3*sin(theta0)**2 + lever**2*a1**4*a2**2 + 2*lever**2*a1**3*a2**3*sin(theta0)**2 + lever**2*a1**2*a2**4 - a1**4*a2**4) - sqrt(-2*lever**8*sin(theta0)**2 + lever**8 + 2*lever**6*a1**2*sin(theta0)**2 - lever**6*a1**2 + 2*lever**6*a1*a2*sin(theta0)**2 + 2*lever**6*a2**2*sin(theta0)**2 - lever**6*a2**2 - 2*lever**4*a1**3*a2*sin(theta0)**2 - 2*lever**4*a1**2*a2**2*sin(theta0)**2 - 2*lever**4*a1*a2**3*sin(theta0)**2 + lever**2*a1**4*a2**2 + 2*lever**2*a1**3*a2**3*sin(theta0)**2 + lever**2*a1**2*a2**4 - a1**4*a2**4))*sin(theta0)/(sqrt(lever**2 - a2**2)*(lever**4 - a1**2*a2**2)) + + jy1 = 4*lever*a1*a2**2*(lever**5*sin(theta0) - lever**3*a1**2*sin(theta0) - lever**3*a1*a2*sin(theta0) + lever*a1**3*a2*sin(theta0) - a1*sqrt(-2*lever**8*sin(theta0)**2 + lever**8 + 2*lever**6*a1**2*sin(theta0)**2 - lever**6*a1**2 + 2*lever**6*a1*a2*sin(theta0)**2 + 2*lever**6*a2**2*sin(theta0)**2 - lever**6*a2**2 - 2*lever**4*a1**3*a2*sin(theta0)**2 - 2*lever**4*a1**2*a2**2*sin(theta0)**2 - 2*lever**4*a1*a2**3*sin(theta0)**2 + lever**2*a1**4*a2**2 + 2*lever**2*a1**3*a2**3*sin(theta0)**2 + lever**2*a1**2*a2**4 - a1**4*a2**4))*sin(theta0)/(sqrt(lever**2 - a1**2)*(lever**4 - a1**2*a2**2)**2) + 2*lever*a1*(lever**5*sin(theta0) - lever**3*a1**2*sin(theta0) - lever**3*a1*a2*sin(theta0) + lever*a1**3*a2*sin(theta0) - a1*sqrt(-2*lever**8*sin(theta0)**2 + lever**8 + 2*lever**6*a1**2*sin(theta0)**2 - lever**6*a1**2 + 2*lever**6*a1*a2*sin(theta0)**2 + 2*lever**6*a2**2*sin(theta0)**2 - lever**6*a2**2 - 2*lever**4*a1**3*a2*sin(theta0)**2 - 2*lever**4*a1**2*a2**2*sin(theta0)**2 - 2*lever**4*a1*a2**3*sin(theta0)**2 + lever**2*a1**4*a2**2 + 2*lever**2*a1**3*a2**3*sin(theta0)**2 + lever**2*a1**2*a2**4 - a1**4*a2**4))*sin(theta0)/((lever**2 - a1**2)**(3/2)*(lever**4 - a1**2*a2**2)) + 2*lever*(-2*lever**3*a1*sin(theta0) - lever**3*a2*sin(theta0) + 3*lever*a1**2*a2*sin(theta0) - a1*(2*lever**6*a1*sin(theta0)**2 - lever**6*a1 + lever**6*a2*sin(theta0)**2 - 3*lever**4*a1**2*a2*sin(theta0)**2 - 2*lever**4*a1*a2**2*sin(theta0)**2 - lever**4*a2**3*sin(theta0)**2 + 2*lever**2*a1**3*a2**2 + 3*lever**2*a1**2*a2**3*sin(theta0)**2 + lever**2*a1*a2**4 - 2*a1**3*a2**4)/sqrt(-2*lever**8*sin(theta0)**2 + lever**8 + 2*lever**6*a1**2*sin(theta0)**2 - lever**6*a1**2 + 2*lever**6*a1*a2*sin(theta0)**2 + 2*lever**6*a2**2*sin(theta0)**2 - lever**6*a2**2 - 2*lever**4*a1**3*a2*sin(theta0)**2 - 2*lever**4*a1**2*a2**2*sin(theta0)**2 - 2*lever**4*a1*a2**3*sin(theta0)**2 + lever**2*a1**4*a2**2 + 2*lever**2*a1**3*a2**3*sin(theta0)**2 + lever**2*a1**2*a2**4 - a1**4*a2**4) - sqrt(-2*lever**8*sin(theta0)**2 + lever**8 + 2*lever**6*a1**2*sin(theta0)**2 - lever**6*a1**2 + 2*lever**6*a1*a2*sin(theta0)**2 + 2*lever**6*a2**2*sin(theta0)**2 - lever**6*a2**2 - 2*lever**4*a1**3*a2*sin(theta0)**2 - 2*lever**4*a1**2*a2**2*sin(theta0)**2 - 2*lever**4*a1*a2**3*sin(theta0)**2 + lever**2*a1**4*a2**2 + 2*lever**2*a1**3*a2**3*sin(theta0)**2 + lever**2*a1**2*a2**4 - a1**4*a2**4))*sin(theta0)/(sqrt(lever**2 - a1**2)*(lever**4 - a1**2*a2**2)) + jy2 = 4*lever*a1**2*a2*(lever**5*sin(theta0) - lever**3*a1**2*sin(theta0) - lever**3*a1*a2*sin(theta0) + lever*a1**3*a2*sin(theta0) - a1*sqrt(-2*lever**8*sin(theta0)**2 + lever**8 + 2*lever**6*a1**2*sin(theta0)**2 - lever**6*a1**2 + 2*lever**6*a1*a2*sin(theta0)**2 + 2*lever**6*a2**2*sin(theta0)**2 - lever**6*a2**2 - 2*lever**4*a1**3*a2*sin(theta0)**2 - 2*lever**4*a1**2*a2**2*sin(theta0)**2 - 2*lever**4*a1*a2**3*sin(theta0)**2 + lever**2*a1**4*a2**2 + 2*lever**2*a1**3*a2**3*sin(theta0)**2 + lever**2*a1**2*a2**4 - a1**4*a2**4))*sin(theta0)/(sqrt(lever**2 - a1**2)*(lever**4 - a1**2*a2**2)**2) + 2*lever*(-lever**3*a1*sin(theta0) + lever*a1**3*sin(theta0) - a1*(lever**6*a1*sin(theta0)**2 + 2*lever**6*a2*sin(theta0)**2 - lever**6*a2 - lever**4*a1**3*sin(theta0)**2 - 2*lever**4*a1**2*a2*sin(theta0)**2 - 3*lever**4*a1*a2**2*sin(theta0)**2 + lever**2*a1**4*a2 + 3*lever**2*a1**3*a2**2*sin(theta0)**2 + 2*lever**2*a1**2*a2**3 - 2*a1**4*a2**3)/sqrt(-2*lever**8*sin(theta0)**2 + lever**8 + 2*lever**6*a1**2*sin(theta0)**2 - lever**6*a1**2 + 2*lever**6*a1*a2*sin(theta0)**2 + 2*lever**6*a2**2*sin(theta0)**2 - lever**6*a2**2 - 2*lever**4*a1**3*a2*sin(theta0)**2 - 2*lever**4*a1**2*a2**2*sin(theta0)**2 - 2*lever**4*a1*a2**3*sin(theta0)**2 + lever**2*a1**4*a2**2 + 2*lever**2*a1**3*a2**3*sin(theta0)**2 + lever**2*a1**2*a2**4 - a1**4*a2**4))*sin(theta0)/(sqrt(lever**2 - a1**2)*(lever**4 - a1**2*a2**2)) + + jz1 = 4*lever*a1*a2**2*(lever*(lever**4 - a1**2*a2**2)*(a1 + a2)*sin(theta0) + (lever**2 + a1*a2)*sqrt(lever**2*(lever**2*a1 + lever**2*a2 - a1**2*a2 - a1*a2**2)**2*sin(theta0)**2 + (-lever**4 + a1**2*a2**2)*(-2*lever**4*cos(theta0)**2 + lever**4 + lever**2*a1**2*cos(theta0)**2 + lever**2*a2**2*cos(theta0)**2 - a1**2*a2**2)))*sin(theta0)/((lever**2 + a1*a2)*(lever**4 - a1**2*a2**2)**2) - 2*lever*a2*(lever*(lever**4 - a1**2*a2**2)*(a1 + a2)*sin(theta0) + (lever**2 + a1*a2)*sqrt(lever**2*(lever**2*a1 + lever**2*a2 - a1**2*a2 - a1*a2**2)**2*sin(theta0)**2 + (-lever**4 + a1**2*a2**2)*(-2*lever**4*cos(theta0)**2 + lever**4 + lever**2*a1**2*cos(theta0)**2 + lever**2*a2**2*cos(theta0)**2 - a1**2*a2**2)))*sin(theta0)/((lever**2 + a1*a2)**2*(lever**4 - a1**2*a2**2)) + 2*lever*(-2*lever*a1*a2**2*(a1 + a2)*sin(theta0) + lever*(lever**4 - a1**2*a2**2)*sin(theta0) + a2*sqrt(lever**2*(lever**2*a1 + lever**2*a2 - a1**2*a2 - a1*a2**2)**2*sin(theta0)**2 + (-lever**4 + a1**2*a2**2)*(-2*lever**4*cos(theta0)**2 + lever**4 + lever**2*a1**2*cos(theta0)**2 + lever**2*a2**2*cos(theta0)**2 - a1**2*a2**2)) + (lever**2 + a1*a2)*(lever**2*(2*lever**2 - 4*a1*a2 - 2*a2**2)*(lever**2*a1 + lever**2*a2 - a1**2*a2 - a1*a2**2)*sin(theta0)**2/2 + a1*a2**2*(-2*lever**4*cos(theta0)**2 + lever**4 + lever**2*a1**2*cos(theta0)**2 + lever**2*a2**2*cos(theta0)**2 - a1**2*a2**2) + (-lever**4 + a1**2*a2**2)*(2*lever**2*a1*cos(theta0)**2 - 2*a1*a2**2)/2)/sqrt(lever**2*(lever**2*a1 + lever**2*a2 - a1**2*a2 - a1*a2**2)**2*sin(theta0)**2 + (-lever**4 + a1**2*a2**2)*(-2*lever**4*cos(theta0)**2 + lever**4 + lever**2*a1**2*cos(theta0)**2 + lever**2*a2**2*cos(theta0)**2 - a1**2*a2**2)))*sin(theta0)/((lever**2 + a1*a2)*(lever**4 - a1**2*a2**2)) + jz2 = 4*lever*a1**2*a2*(lever*(lever**4 - a1**2*a2**2)*(a1 + a2)*sin(theta0) + (lever**2 + a1*a2)*sqrt(lever**2*(lever**2*a1 + lever**2*a2 - a1**2*a2 - a1*a2**2)**2*sin(theta0)**2 + (-lever**4 + a1**2*a2**2)*(-2*lever**4*cos(theta0)**2 + lever**4 + lever**2*a1**2*cos(theta0)**2 + lever**2*a2**2*cos(theta0)**2 - a1**2*a2**2)))*sin(theta0)/((lever**2 + a1*a2)*(lever**4 - a1**2*a2**2)**2) - 2*lever*a1*(lever*(lever**4 - a1**2*a2**2)*(a1 + a2)*sin(theta0) + (lever**2 + a1*a2)*sqrt(lever**2*(lever**2*a1 + lever**2*a2 - a1**2*a2 - a1*a2**2)**2*sin(theta0)**2 + (-lever**4 + a1**2*a2**2)*(-2*lever**4*cos(theta0)**2 + lever**4 + lever**2*a1**2*cos(theta0)**2 + lever**2*a2**2*cos(theta0)**2 - a1**2*a2**2)))*sin(theta0)/((lever**2 + a1*a2)**2*(lever**4 - a1**2*a2**2)) + 2*lever*(-2*lever*a1**2*a2*(a1 + a2)*sin(theta0) + lever*(lever**4 - a1**2*a2**2)*sin(theta0) + a1*sqrt(lever**2*(lever**2*a1 + lever**2*a2 - a1**2*a2 - a1*a2**2)**2*sin(theta0)**2 + (-lever**4 + a1**2*a2**2)*(-2*lever**4*cos(theta0)**2 + lever**4 + lever**2*a1**2*cos(theta0)**2 + lever**2*a2**2*cos(theta0)**2 - a1**2*a2**2)) + (lever**2 + a1*a2)*(lever**2*(2*lever**2 - 2*a1**2 - 4*a1*a2)*(lever**2*a1 + lever**2*a2 - a1**2*a2 - a1*a2**2)*sin(theta0)**2/2 + a1**2*a2*(-2*lever**4*cos(theta0)**2 + lever**4 + lever**2*a1**2*cos(theta0)**2 + lever**2*a2**2*cos(theta0)**2 - a1**2*a2**2) + (-lever**4 + a1**2*a2**2)*(2*lever**2*a2*cos(theta0)**2 - 2*a1**2*a2)/2)/sqrt(lever**2*(lever**2*a1 + lever**2*a2 - a1**2*a2 - a1*a2**2)**2*sin(theta0)**2 + (-lever**4 + a1**2*a2**2)*(-2*lever**4*cos(theta0)**2 + lever**4 + lever**2*a1**2*cos(theta0)**2 + lever**2*a2**2*cos(theta0)**2 - a1**2*a2**2)))*sin(theta0)/((lever**2 + a1*a2)*(lever**4 - a1**2*a2**2)) + + return dict( + jx1=jx1, jx2=jx2, + jy1=jy1, jy2=jy2, + jz1=jz1, jz2=jz2, + ) + + +def jacobian_ori(): + """ + From https://colab.research.google.com/drive/16M2i_O4wQAMEVkaWrUnTU6smszJXCJpj + """ + + jrx1 = lever ** 2 + jrx2 = lever ** 2 + + jry1 = lever ** 2 + jry2 = lever ** 2 + + jrz1 = lever ** 2 + jrz2 = lever ** 2 + + return dict( + jrx1=jrx1, jrx2=jrx2, + jry1=jry1, jry2=jry2, + jrz1=jrz1, jrz2=jrz2, + ) + + +def radius(): + return dict(radius=2 * sin(theta0) * lever) + + +def actuator_offset(): + return dict(actuator_offset=asin(theta0)) + + +test_mode = False + +if not test_mode and __name__ == '__main__': + + output_dir = os.path.expandvars("$simox__PATH/VirtualRobot/Nodes/HemisphereJoint/") + assert os.path.isdir(output_dir) + name = "Expressions" + + pos = fk_pos() + ori = fk_ori(pos) + jac_pos = jacobian_pos() + jac_ori = jacobian_ori() + + cpp = SympyToCpp( + name=name, + function_args=[a1, a2, lever, theta0], + function_results=dict( + # ez=pos["ez"], + **pos, + **ori, + **jac_pos, + **jac_ori, + # No need to re-evaluate these each time. + # **radius(), + # **actuator_offset(), + ) + ) + cpp.build() + + header_path = os.path.join(output_dir, cpp.name + ".h") + source_path = os.path.join(output_dir, cpp.name + ".cpp") + + header_lines = cpp.make_header_lines() + source_lines = cpp.make_source_lines() + + print("Declaration:") + print(cpp.format_lines(header_lines, line_numbers=True)) + print("Implementation:") + print(cpp.format_lines(source_lines, line_numbers=True)) + + print("Writing files...") + print(f"- {header_path}") + print(cpp.write_lines(header_lines, header_path)) + print(f"- {source_path}") + print(cpp.write_lines(source_lines, source_path)) + + print("Done.") + + +if test_mode and __name__ == '__main__': + import numpy as np + from numpy import sin, cos, sqrt + + lever = 1 + theta0 = np.deg2rad(25) + actuator_offset = np.arcsin(theta0) + + a = np.array([0., 0.]) + a = a + actuator_offset + a1, a2 = a + + pos = fk_pos() + print(pos) + diff --git a/python/hemisphere-joint-demo/hemisphere_joint_demo/app/hemisphere_joint_demo_run.py b/python/hemisphere-joint-demo/hemisphere_joint_demo/app/hemisphere_joint_demo_run.py new file mode 100755 index 0000000000000000000000000000000000000000..5b8197d3705aafd5ba4d76d3ae82ca07d9535957 --- /dev/null +++ b/python/hemisphere-joint-demo/hemisphere_joint_demo/app/hemisphere_joint_demo_run.py @@ -0,0 +1,277 @@ +import enum +import os +import numpy as np +import typing as ty + +from armarx import arviz as viz +from armarx import remote_gui as rg + +from armarx.remote_gui.widgets.ndarray import NdArrayWidget + + +def spherical2cartesian( + spherical: ty.Union[ty.List, np.ndarray], +) -> np.ndarray: + spherical = np.array(spherical) + + assert spherical.shape[-1] == 3 + radius = spherical[..., 0] + azim = spherical[..., 1] + elev = spherical[..., 2] + inclination = np.pi / 2 - elev + sin_inclination = np.sin(inclination) + + cartesian = spherical.astype(np.float).copy() + cartesian[..., 0] = radius * sin_inclination * np.cos(azim) # x + cartesian[..., 1] = radius * sin_inclination * np.sin(azim) # y + cartesian[..., 2] = radius * np.cos(inclination) # z + return cartesian + + +class Data: + class Mode(enum.IntEnum): + FK = 0 + IK = 1 + + def __init__(self): + super().__init__() + + self.lever = 1 + self.theta_0 = np.deg2rad(25) + self.radius = 2 * np.sin(self.theta_0) * self.lever + self.actuator_offset = np.arcsin(self.theta_0) + + self.mode = self.Mode.FK + + self.actuator_pos = np.zeros(2) + # self.actuator_pos[:] = (0.5, 0.5) + self.actuator_vel = np.zeros(2) + + self.eef_pos = np.zeros(3) + self.eef_ori = np.identity(3) + + self.eef_vel = np.zeros(3) + self.eef_rot_vel = np.zeros(3) + + self.fk() + + def fk(self): + print("-" * 50) + a1, a2 = self.actuator_pos + self.actuator_offset + + # KIT-Wrist constants + lever = self.lever + theta_0 = self.theta_0 + radius = self.radius + print(f"Lever: {lever}") + print(f"theta0: {theta_0}") + print(f"radius: {radius}") + print(f"actuator_offset: {self.actuator_offset}") + print(f"(a1, a2) = ({a1}, {a2})") + + do_fk_pos_azim_zenith = False + if do_fk_pos_azim_zenith: + from hemisphere_joint_demo.equations import f_azimuth, f_zenith + + azimuth = f_azimuth(a1=a1, a2=a2, L=lever, T_0=theta_0) + zenith = f_zenith(a1=a1, a2=a2, L=lever, T_0=theta_0) + + elevation = np.deg2rad(90) - zenith + self.eef_pos = spherical2cartesian([radius, azimuth, elevation]) + print(f"(azimuth, zenith) = ({azimuth:.3f}, {zenith:.3f})") + + do_fk = True + if do_fk: + from hemisphere_joint_demo.equations import fk_pos + self.eef_pos = fk_pos(a1, a2, L=lever, T_0=theta_0) + + do_fk_ori = True + if do_fk_ori: + from hemisphere_joint_demo.equations import fk_ori + self.eef_ori = fk_ori(self.eef_pos[0], self.eef_pos[1], L=lever, T_0=theta_0) + + do_jac = True + if do_jac: + from hemisphere_joint_demo.equations import jacobian + jac = jacobian(a1=a1, a2=a2, L=lever, T_0=theta_0) + print(f"Jacobian: \n{np.round(jac, 3).tolist()}") + # print(f"Jacobian: \n{jac}") + + vel = jac @ self.actuator_vel + self.eef_vel = vel[:3] + self.eef_rot_vel = vel[3:] + + print(f"EEF pos = {np.round(self.eef_pos, 3)} (norm = {np.linalg.norm(self.eef_pos):.3f})") + print(f"EEF vel = {np.round(self.eef_vel, 3)} (norm = {np.linalg.norm(self.eef_vel):.3f})") + + print(f"EEF ori = \n{np.round(self.eef_ori, 3)}") + print(f"EEF rot vel = {np.round(self.eef_rot_vel, 3)} (norm = {np.linalg.norm(self.eef_rot_vel):.3f})") + + def ik(self): + from hemisphere_joint_demo.equations import fk_pos, ik + + print("-" * 50) + + eef_pos = self.radius * self.eef_pos / np.linalg.norm(self.eef_pos) + print(f"EEF pos = {np.round(eef_pos, 3)} (norm = {np.linalg.norm(eef_pos):.3f})") + + ex, ey, ez = eef_pos + a1, a2 = ik(ex, ey, L=self.lever, T_0=self.theta_0) + self.actuator_pos[:] = [a1, a2] + + fk_eef_pos = fk_pos(a1, a2, L=self.lever, T_0=self.theta_0) + error = fk_eef_pos - eef_pos + print(f"FK(a..) = {np.round(fk_eef_pos, 3)}") + print(f"Error = {np.round(error, 5)}, |E| = {np.linalg.norm(error):.5f}") + + print(f"Actuator pos = {self.actuator_pos}") + + +class Visu: + + def __init__(self, data: Data): + self.data = data + + def visualize(self, stage: viz.Stage): + self.vis_actuators(stage.layer("Actuators")) + self.vis_eef(stage.layer("EEF Pos")) + self.vis_sphere(stage.layer("Sphere")) + + stage.origin_layer(scale=3e-3) + + def vis_actuators(self, layer: viz.Layer): + def angle_to_pos(angle): + return ground_radius * np.array([np.cos(angle), np.sin(angle), 0]) + + ground_radius = 1.5 + z = np.array([0, 0, 1]) + for i, (pos, vel) in enumerate(zip(self.data.actuator_pos, self.data.actuator_vel)): + # Position + if pos == 0: + pos = 1e-3 + angle = i * np.pi / 2 + start = angle_to_pos(angle) + end = start + pos * 1 * z + layer.add(viz.Arrow(f"actuator pos {i}", from_to=(start, end), + width=0.05, color=(255, 192, 0))) + + # Velocity + if vel == 0: + vel = 1e-3 + angle += 1 / 32 * 2 * np.pi + start = angle_to_pos(angle) + end = start + vel * 1 * z + layer.add(viz.Arrow(f"actuator vel {i}", from_to=(start, end), width=0.02, color=(255, 32, 0))) + + def vis_eef(self, layer: viz.Layer): + # Env + pos = self.data.eef_pos + vel = self.data.eef_vel + + layer.add(viz.Sphere("eef pos", position=pos, radius=0.05, color=(0, 0, 255))) + layer.add(viz.Pose("eef pose", position=pos, orientation=self.data.eef_ori, scale=3e-3)) + layer.add(viz.Arrow("eef vector", from_to=((0, 0, 0), pos), width=0.02, color=(30, 30, 70))) + layer.add(viz.Arrow("eef out vector", from_to=(pos, pos + .5 * self.data.eef_ori @ np.array([0, 0, 1])), + color=(0, 0, 255), width=0.02)) + + if np.linalg.norm(vel) >= 1e-3: + speed = 1.0 + vel_end = pos + speed * vel + layer.add(viz.Arrow("eef velocity", from_to=(pos, vel_end), width=0.02, color=(0, 255, 70))) + + def vis_sphere(self, layer: viz.Layer): + layer.add(viz.Cylinder("circle", from_to=((0, 0, 0), (0, 0, 1e-3)), + radius=1, color=(255, 255, 0, 255))) + layer.add(viz.Sphere("sphere", radius=self.data.radius, color=(255, 255, 0, 64))) + + +class WidgetsTab(rg.Tab): + + def __init__( + self, + data: Data, + visu: Visu, + tag=None, + arviz: ty.Optional[viz.Client] = None): + super().__init__(tag or TAG) + + self.data = data + self.visu = visu + self.arviz = arviz or viz.Client(tag) + + self.mode = rg.ComboBox(options=[m.name for m in Data.Mode]) + + limit = 0.7 + self.actuator_pos = NdArrayWidget(data.actuator_pos, row_vector=True, + range_min=-limit, range_max=limit) + self.actuator_vel = NdArrayWidget(data.actuator_vel, row_vector=True, + range_min=-limit, range_max=limit) + + self.eef_pos = NdArrayWidget(data.eef_pos, row_vector=True, range_min=-self.data.radius, range_max=self.data.radius) + self.eef_vel = NdArrayWidget(data.eef_vel, row_vector=True, range_min=-1, range_max=1) + + def create_widget_tree(self) -> rg.GridLayout: + layout = rg.GridLayout() + row = 0 + + layout.add(rg.Label("Mode:"), (row, 0)).add(self.mode, (row, 1)) + row += 1 + + layout.add(rg.Label("Actuator Pos:"), (row, 0)).add(self.actuator_pos.create_tree(), (row, 1)) + row += 1 + layout.add(rg.Label("Actuator Vel:"), (row, 0)).add(self.actuator_vel.create_tree(), (row, 1)) + row += 1 + + layout.add(rg.Label("EEF Pos:"), (row, 0)).add(self.eef_pos.create_tree(), (row, 1)) + row += 1 + layout.add(rg.Label("EEF Vel:"), (row, 0)).add(self.eef_vel.create_tree(), (row, 1)) + row += 1 + + return layout + + def update(self): + updated = False + + if self.mode.has_value_changed(): + self.data.mode = Data.Mode[self.mode.value] + updated = True + + if self.data.mode == Data.Mode.FK: + if updated or self.actuator_pos.has_any_changed() or self.actuator_vel.has_any_changed(): + self.data.actuator_pos = self.actuator_pos.get_array() + self.data.actuator_vel = self.actuator_vel.get_array() + self.data.fk() + updated = True + + if self.data.mode == Data.Mode.IK: + if updated or self.eef_pos.has_any_changed(): + self.data.eef_pos = self.eef_pos.get_array() + self.data.ik() + updated = True + + if updated: + try: + with self.arviz.begin_stage(commit_on_exit=True) as stage: + self.visu.visualize(stage) + except np.linalg.LinAlgError as e: + print(e) + + +if __name__ == "__main__": + TAG = os.path.basename(__file__) + + data = Data() + visu = Visu(data) + arviz = viz.Client(TAG) + + with arviz.begin_stage(commit_on_exit=True) as stage: + visu.visualize(stage) + + rg_client = rg.Client() + + widgets_tab = WidgetsTab(data=data, visu=visu, tag=TAG) + rg_client.add_tab(widgets_tab) + rg_client.receive_updates() + rg_client.receive_updates() + + rg_client.update_loop(lambda: widgets_tab.update(), block=False) 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 0000000000000000000000000000000000000000..839988dbd2c8463c11d4730aab8496489e6cfefb --- /dev/null +++ b/python/hemisphere-joint-demo/hemisphere_joint_demo/equations.py @@ -0,0 +1,2962 @@ +import numpy as np + +from numpy import pi, sin, cos, sqrt, arcsin, arctan2 + +from .terms import CommonTerms + + +def fk_pos(a1, a2, 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, a1=a1, a2=a2) + + ex = 2 * L * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_03) * t.sin_t0 / ( + sqrt(t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq) + ey = 2 * L * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_03) * t.sin_t0 / ( + sqrt(t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq) + ez = 2 * L * (L * t.l_p4_minus_a1_sq_mul_a2_sq * (a1 + a2) * t.sin_t0 + (t.l_p2 + a1 * a2) * sqrt( + t.l_p2 * (t.l_p2 * a1 + t.l_p2 * a2 - t.a1_p2 * a2 - a1 * t.a2_p2) ** 2 * t.sin_t0_sq + ( + -t.l_p4 + t.a1_p2 * t.a2_p2) * ( + -2 * t.l_p4 * t.cos_t0_sq + t.l_p4 + t.l_p2 * t.a1_p2 * t.cos_t0_sq + t.l_p2 * t.a2_p2 * t.cos_t0_sq - t.a1_p2 * t.a2_p2))) * t.sin_t0 / ( + (t.l_p2 + a1 * a2) * t.l_p4_minus_a1_sq_mul_a2_sq) + + return np.array([ex, ey, ez]) + + +def fk_ori(ex, ey, 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, ex=ex, ey=ey) + + # @title Orientation vectors + exx = 1 - t.ex_p2 / t.two_l_p2_mul_sin_t0_sq + exy = -t.ex_mul_ey / t.two_l_p2_mul_sin_t0_sq + exz = -ex * t.sqrt_4_l_p2_mul_sin_t0_sq_mul_ex_sq_mul_ey_sq_div_two_l_p2_mul_sin_t0_sq + + eyx = -t.ex_mul_ey / t.two_l_p2_mul_sin_t0_sq + eyy = 1 - t.ey_p2 / t.two_l_p2_mul_sin_t0_sq + eyz = -ey * t.sqrt_4_l_p2_mul_sin_t0_sq_mul_ex_sq_mul_ey_sq_div_two_l_p2_mul_sin_t0_sq + + ezx = ex * t.sqrt_4_l_p2_mul_sin_t0_sq_mul_ex_sq_mul_ey_sq_div_two_l_p2_mul_sin_t0_sq + ezy = ey * t.sqrt_4_l_p2_mul_sin_t0_sq_mul_ex_sq_mul_ey_sq_div_two_l_p2_mul_sin_t0_sq + ezz = (2 * t.l_p2 - t.ex_p2 / t.sin_t0_sq - t.ey_p2 / t.sin_t0_sq) / (2 * t.l_p2) + + # @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(a1, a2, L, T_0): + """ + From: + https://colab.research.google.com/drive/11faUc8pS1yWxFrnmt05VqpDsqOwEi_dg#scrollTo=xx7_60I1sV9j&line=1&uniqifier=1 + """ + t = CommonTerms(L=L, T_0=T_0, a1=a1, a2=a2) + + # Jacobian of [ex, ey, ez, azimuth, declination] + + jac = np.zeros((5, 2), dtype=float) + + jac[0, 0] = (4 * L * a1 * a2 ** 2 * (L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin(T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin(T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin(T_0) / ( + sqrt(L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2) + 2 * L * ( + -L ** 3 * a2 * sin(T_0) + L * a2 ** 3 * sin(T_0) - a2 * ( + 2 * L ** 6 * a1 * sin(T_0) ** 2 - L ** 6 * a1 + L ** 6 * a2 * sin( + T_0) ** 2 - 3 * L ** 4 * a1 ** 2 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 2 * sin(T_0) ** 2 - L ** 4 * a2 ** 3 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 2 + 3 * L ** 2 * a1 ** 2 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 * a2 ** 4 - 2 * a1 ** 3 * a2 ** 4) / sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin(T_0) / ( + sqrt(L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2))) + + jac[0, 1] = (4 * L * a1 ** 2 * a2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin(T_0) / ( + sqrt(L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2) + 2 * L * a2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin(T_0) / ( + (L ** 2 - a2 ** 2) ** (3 / 2) * (L ** 4 - a1 ** 2 * a2 ** 2)) + 2 * L * ( + -L ** 3 * a1 * sin(T_0) - 2 * L ** 3 * a2 * sin(T_0) + 3 * L * a1 * a2 ** 2 * sin( + T_0) - a2 * (L ** 6 * a1 * sin(T_0) ** 2 + 2 * L ** 6 * a2 * sin( + T_0) ** 2 - L ** 6 * a2 - L ** 4 * a1 ** 3 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 * sin(T_0) ** 2 - 3 * L ** 4 * a1 * a2 ** 2 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 + 3 * L ** 2 * a1 ** 3 * a2 ** 2 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 2 * a2 ** 3 - 2 * a1 ** 4 * a2 ** 3) / sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4) - sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin(T_0) / ( + sqrt(L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2))) + + jac[1] = [4 * L * a1 * a2 ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin(T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin(T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin(T_0) / (sqrt( + L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2) + 2 * L * a1 * (L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt(-2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin(T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin(T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin(T_0) / ((L ** 2 - a1 ** 2) ** ( + 3 / 2) * (L ** 4 - a1 ** 2 * a2 ** 2)) + 2 * L * (-2 * L ** 3 * a1 * sin( + T_0) - L ** 3 * a2 * sin(T_0) + 3 * L * a1 ** 2 * a2 * sin(T_0) - a1 * (2 * L ** 6 * a1 * sin( + T_0) ** 2 - L ** 6 * a1 + L ** 6 * a2 * sin(T_0) ** 2 - 3 * L ** 4 * a1 ** 2 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 2 * sin(T_0) ** 2 - L ** 4 * a2 ** 3 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 2 + 3 * L ** 2 * a1 ** 2 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 * a2 ** 4 - 2 * a1 ** 3 * a2 ** 4) / sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin(T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin(T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4) - sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin(T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin(T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin(T_0) / (sqrt( + L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2)), + + 4 * L * a1 ** 2 * a2 * (L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt(-2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin(T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin(T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin(T_0) / (sqrt(L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) + 2 * L * (-L ** 3 * a1 * sin(T_0) + L * a1 ** 3 * sin( + T_0) - a1 * (L ** 6 * a1 * sin(T_0) ** 2 + 2 * L ** 6 * a2 * sin( + T_0) ** 2 - L ** 6 * a2 - L ** 4 * a1 ** 3 * sin(T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 * sin( + T_0) ** 2 - 3 * L ** 4 * a1 * a2 ** 2 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 + 3 * L ** 2 * a1 ** 3 * a2 ** 2 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 2 * a2 ** 3 - 2 * a1 ** 4 * a2 ** 3) / sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin(T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin(T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin(T_0) / (sqrt( + L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2))] + + jac[2] = [4 * L * a1 * a2 ** 2 * ( + L * (L ** 4 - a1 ** 2 * a2 ** 2) * (a1 + a2) * sin(T_0) + (L ** 2 + a1 * a2) * sqrt( + L ** 2 * (L ** 2 * a1 + L ** 2 * a2 - a1 ** 2 * a2 - a1 * a2 ** 2) ** 2 * sin(T_0) ** 2 + ( + -L ** 4 + a1 ** 2 * a2 ** 2) * ( + -2 * L ** 4 * cos(T_0) ** 2 + L ** 4 + L ** 2 * a1 ** 2 * cos( + T_0) ** 2 + L ** 2 * a2 ** 2 * cos(T_0) ** 2 - a1 ** 2 * a2 ** 2))) * sin(T_0) / ( + (L ** 2 + a1 * a2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 2 * L * a2 * ( + L * (L ** 4 - a1 ** 2 * a2 ** 2) * ( + a1 + a2) * sin(T_0) + ( + L ** 2 + a1 * a2) * sqrt( + L ** 2 * ( + L ** 2 * a1 + L ** 2 * a2 - a1 ** 2 * a2 - a1 * a2 ** 2) ** 2 * sin( + T_0) ** 2 + ( + -L ** 4 + a1 ** 2 * a2 ** 2) * ( + -2 * L ** 4 * cos( + T_0) ** 2 + L ** 4 + L ** 2 * a1 ** 2 * cos( + T_0) ** 2 + L ** 2 * a2 ** 2 * cos( + T_0) ** 2 - a1 ** 2 * a2 ** 2))) * sin( + T_0) / ((L ** 2 + a1 * a2) ** 2 * (L ** 4 - a1 ** 2 * a2 ** 2)) + 2 * L * (-2 * L * a1 * a2 ** 2 * ( + a1 + a2) * sin(T_0) + L * (L ** 4 - a1 ** 2 * a2 ** 2) * sin(T_0) + a2 * sqrt( + L ** 2 * (L ** 2 * a1 + L ** 2 * a2 - a1 ** 2 * a2 - a1 * a2 ** 2) ** 2 * sin(T_0) ** 2 + ( + -L ** 4 + a1 ** 2 * a2 ** 2) * (-2 * L ** 4 * cos(T_0) ** 2 + L ** 4 + L ** 2 * a1 ** 2 * cos( + T_0) ** 2 + L ** 2 * a2 ** 2 * cos(T_0) ** 2 - a1 ** 2 * a2 ** 2)) + (L ** 2 + a1 * a2) * ( + L ** 2 * ( + 2 * L ** 2 - 4 * a1 * a2 - 2 * a2 ** 2) * ( + L ** 2 * a1 + L ** 2 * a2 - a1 ** 2 * a2 - a1 * a2 ** 2) * sin( + T_0) ** 2 / 2 + a1 * a2 ** 2 * ( + -2 * L ** 4 * cos( + T_0) ** 2 + L ** 4 + L ** 2 * a1 ** 2 * cos( + T_0) ** 2 + L ** 2 * a2 ** 2 * cos( + T_0) ** 2 - a1 ** 2 * a2 ** 2) + ( + -L ** 4 + a1 ** 2 * a2 ** 2) * ( + 2 * L ** 2 * a1 * cos( + T_0) ** 2 - 2 * a1 * a2 ** 2) / 2) / sqrt( + L ** 2 * (L ** 2 * a1 + L ** 2 * a2 - a1 ** 2 * a2 - a1 * a2 ** 2) ** 2 * sin(T_0) ** 2 + ( + -L ** 4 + a1 ** 2 * a2 ** 2) * (-2 * L ** 4 * cos(T_0) ** 2 + L ** 4 + L ** 2 * a1 ** 2 * cos( + T_0) ** 2 + L ** 2 * a2 ** 2 * cos(T_0) ** 2 - a1 ** 2 * a2 ** 2))) * sin(T_0) / ( + (L ** 2 + a1 * a2) * ( + L ** 4 - a1 ** 2 * a2 ** 2)), + + + 4 * L * a1 ** 2 * a2 * ( + L * (L ** 4 - a1 ** 2 * a2 ** 2) * ( + a1 + a2) * sin(T_0) + ( + L ** 2 + a1 * a2) * sqrt( + L ** 2 * ( + L ** 2 * a1 + L ** 2 * a2 - a1 ** 2 * a2 - a1 * a2 ** 2) ** 2 * sin( + T_0) ** 2 + ( + -L ** 4 + a1 ** 2 * a2 ** 2) * ( + -2 * L ** 4 * cos( + T_0) ** 2 + L ** 4 + L ** 2 * a1 ** 2 * cos( + T_0) ** 2 + L ** 2 * a2 ** 2 * cos( + T_0) ** 2 - a1 ** 2 * a2 ** 2))) * sin( + T_0) / ((L ** 2 + a1 * a2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 2 * L * a1 * ( + L * (L ** 4 - a1 ** 2 * a2 ** 2) * ( + a1 + a2) * sin(T_0) + ( + L ** 2 + a1 * a2) * sqrt( + L ** 2 * ( + L ** 2 * a1 + L ** 2 * a2 - a1 ** 2 * a2 - a1 * a2 ** 2) ** 2 * sin( + T_0) ** 2 + ( + -L ** 4 + a1 ** 2 * a2 ** 2) * ( + -2 * L ** 4 * cos( + T_0) ** 2 + L ** 4 + L ** 2 * a1 ** 2 * cos( + T_0) ** 2 + L ** 2 * a2 ** 2 * cos( + T_0) ** 2 - a1 ** 2 * a2 ** 2))) * sin( + T_0) / ((L ** 2 + a1 * a2) ** 2 * ( + L ** 4 - a1 ** 2 * a2 ** 2)) + 2 * L * ( + -2 * L * a1 ** 2 * a2 * ( + a1 + a2) * sin(T_0) + L * ( + L ** 4 - a1 ** 2 * a2 ** 2) * sin( + T_0) + a1 * sqrt(L ** 2 * ( + L ** 2 * a1 + L ** 2 * a2 - a1 ** 2 * a2 - a1 * a2 ** 2) ** 2 * sin( + T_0) ** 2 + ( + -L ** 4 + a1 ** 2 * a2 ** 2) * ( + -2 * L ** 4 * cos( + T_0) ** 2 + L ** 4 + L ** 2 * a1 ** 2 * cos( + T_0) ** 2 + L ** 2 * a2 ** 2 * cos( + T_0) ** 2 - a1 ** 2 * a2 ** 2)) + ( + L ** 2 + a1 * a2) * ( + L ** 2 * ( + 2 * L ** 2 - 2 * a1 ** 2 - 4 * a1 * a2) * ( + L ** 2 * a1 + L ** 2 * a2 - a1 ** 2 * a2 - a1 * a2 ** 2) * sin( + T_0) ** 2 / 2 + a1 ** 2 * a2 * ( + -2 * L ** 4 * cos( + T_0) ** 2 + L ** 4 + L ** 2 * a1 ** 2 * cos( + T_0) ** 2 + L ** 2 * a2 ** 2 * cos( + T_0) ** 2 - a1 ** 2 * a2 ** 2) + ( + -L ** 4 + a1 ** 2 * a2 ** 2) * ( + 2 * L ** 2 * a2 * cos( + T_0) ** 2 - 2 * a1 ** 2 * a2) / 2) / sqrt( + L ** 2 * ( + L ** 2 * a1 + L ** 2 * a2 - a1 ** 2 * a2 - a1 * a2 ** 2) ** 2 * sin( + T_0) ** 2 + ( + -L ** 4 + a1 ** 2 * a2 ** 2) * ( + -2 * L ** 4 * cos( + T_0) ** 2 + L ** 4 + L ** 2 * a1 ** 2 * cos( + T_0) ** 2 + L ** 2 * a2 ** 2 * cos( + T_0) ** 2 - a1 ** 2 * a2 ** 2))) * sin( + T_0) / ((L ** 2 + a1 * a2) * ( + L ** 4 - a1 ** 2 * a2 ** 2))] + + jac[3] = [ + -sqrt(4 * L ** 2 * sin(T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin(T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * ( + 2 * a1 * a2 ** 2 * sqrt(4 * L ** 2 * sin(T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin(T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin(T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) / ( + L * sqrt(L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2 * sin( + T_0)) + a1 * sqrt(4 * L ** 2 * sin(T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin(T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin(T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) / ( + L * (L ** 2 - a1 ** 2) ** (3 / 2) * (L ** 4 - a1 ** 2 * a2 ** 2) * sin( + T_0)) + sqrt(4 * L ** 2 * sin(T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin(T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * ( + -2 * L ** 3 * a1 * sin(T_0) - L ** 3 * a2 * sin( + T_0) + 3 * L * a1 ** 2 * a2 * sin(T_0) - a1 * (2 * L ** 6 * a1 * sin( + T_0) ** 2 - L ** 6 * a1 + L ** 6 * a2 * sin( + T_0) ** 2 - 3 * L ** 4 * a1 ** 2 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 2 * sin( + T_0) ** 2 - L ** 4 * a2 ** 3 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 2 + 3 * L ** 2 * a1 ** 2 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 * a2 ** 4 - 2 * a1 ** 3 * a2 ** 4) / sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4) - sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) / ( + L * sqrt(L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) * sin(T_0)) + ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin(T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * ( + -8 * L ** 2 * a1 * a2 ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin(T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 3) - 8 * L ** 2 * a1 * a2 ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 3) - 4 * L ** 2 * a1 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) ** 2 * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 2 * L ** 2 * ( + -2 * L ** 3 * a2 * sin(T_0) + 2 * L * a2 ** 3 * sin( + T_0) - 2 * a2 * (2 * L ** 6 * a1 * sin( + T_0) ** 2 - L ** 6 * a1 + L ** 6 * a2 * sin( + T_0) ** 2 - 3 * L ** 4 * a1 ** 2 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 2 * sin( + T_0) ** 2 - L ** 4 * a2 ** 3 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 2 + 3 * L ** 2 * a1 ** 2 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 * a2 ** 4 - 2 * a1 ** 3 * a2 ** 4) / sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin(T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin( + T_0) ** 2 / ((L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 2 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * ( + -4 * L ** 3 * a1 * sin(T_0) - 2 * L ** 3 * a2 * sin( + T_0) + 6 * L * a1 ** 2 * a2 * sin(T_0) - 2 * a1 * ( + 2 * L ** 6 * a1 * sin( + T_0) ** 2 - L ** 6 * a1 + L ** 6 * a2 * sin( + T_0) ** 2 - 3 * L ** 4 * a1 ** 2 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 2 * sin( + T_0) ** 2 - L ** 4 * a2 ** 3 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 2 + 3 * L ** 2 * a1 ** 2 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 * a2 ** 4 - 2 * a1 ** 3 * a2 ** 4) / sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4) - 2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) / ( + L * sqrt(L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) * sqrt( + 4 * L ** 2 * sin(T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin(T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ( + (L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * sin( + T_0))) * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) / ( + L * sqrt(L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) * ((4 * L ** 2 * sin( + T_0) ** 2 - 4 * L ** 2 * (L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin(T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * (L ** 5 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 / (L ** 2 * ( + L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2 * sin(T_0) ** 2) + ( + 4 * L ** 2 * sin( + T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / (( + L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / (( + L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 / ( + L ** 2 * ( + L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2 * sin( + T_0) ** 2)) * sin( + T_0)) + sqrt(4 * L ** 2 * sin(T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * ( + 2 * a1 * a2 ** 2 * sqrt(4 * L ** 2 * sin(T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin(T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) / ( + L * sqrt(L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2 * sin( + T_0)) + sqrt(4 * L ** 2 * sin(T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin(T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * ( + -L ** 3 * a2 * sin(T_0) + L * a2 ** 3 * sin(T_0) - a2 * ( + 2 * L ** 6 * a1 * sin(T_0) ** 2 - L ** 6 * a1 + L ** 6 * a2 * sin( + T_0) ** 2 - 3 * L ** 4 * a1 ** 2 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 2 * sin( + T_0) ** 2 - L ** 4 * a2 ** 3 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 2 + 3 * L ** 2 * a1 ** 2 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 * a2 ** 4 - 2 * a1 ** 3 * a2 ** 4) / sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) / ( + L * sqrt(L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) * sin(T_0)) + ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * ( + -8 * L ** 2 * a1 * a2 ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin(T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 3) - 8 * L ** 2 * a1 * a2 ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 3) - 4 * L ** 2 * a1 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) ** 2 * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 2 * L ** 2 * ( + -2 * L ** 3 * a2 * sin(T_0) + 2 * L * a2 ** 3 * sin( + T_0) - 2 * a2 * (2 * L ** 6 * a1 * sin( + T_0) ** 2 - L ** 6 * a1 + L ** 6 * a2 * sin( + T_0) ** 2 - 3 * L ** 4 * a1 ** 2 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 2 * sin( + T_0) ** 2 - L ** 4 * a2 ** 3 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 2 + 3 * L ** 2 * a1 ** 2 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 * a2 ** 4 - 2 * a1 ** 3 * a2 ** 4) / sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin(T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin( + T_0) ** 2 / ((L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 2 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * ( + -4 * L ** 3 * a1 * sin(T_0) - 2 * L ** 3 * a2 * sin( + T_0) + 6 * L * a1 ** 2 * a2 * sin(T_0) - 2 * a1 * ( + 2 * L ** 6 * a1 * sin( + T_0) ** 2 - L ** 6 * a1 + L ** 6 * a2 * sin( + T_0) ** 2 - 3 * L ** 4 * a1 ** 2 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 2 * sin( + T_0) ** 2 - L ** 4 * a2 ** 3 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 2 + 3 * L ** 2 * a1 ** 2 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 * a2 ** 4 - 2 * a1 ** 3 * a2 ** 4) / sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4) - 2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) / ( + L * sqrt(L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) * sqrt( + 4 * L ** 2 * sin(T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin(T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ( + (L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * sin( + T_0))) * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin(T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) / ( + L * sqrt(L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) * ((4 * L ** 2 * sin( + T_0) ** 2 - 4 * L ** 2 * (L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin(T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * (L ** 5 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 / (L ** 2 * ( + L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2 * sin(T_0) ** 2) + ( + 4 * L ** 2 * sin( + T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / (( + L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / (( + L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 / ( + L ** 2 * ( + L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2 * sin( + T_0) ** 2)) * sin( + T_0)), + + -sqrt(4 * L ** 2 * sin(T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin(T_0) ** 2 / ( + (L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * (2 * a1 ** 2 * a2 * sqrt( + 4 * L ** 2 * sin(T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin(T_0) ** 2 / ( + (L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin(T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin(T_0) ** 2 / ( + (L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * (L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt(-2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin(T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin(T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) / (L * sqrt(L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2 * sin(T_0)) + sqrt(4 * L ** 2 * sin(T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin(T_0) ** 2 / ( + (L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * (-L ** 3 * a1 * sin( + T_0) + L * a1 ** 3 * sin(T_0) - a1 * (L ** 6 * a1 * sin(T_0) ** 2 + 2 * L ** 6 * a2 * sin( + T_0) ** 2 - L ** 6 * a2 - L ** 4 * a1 ** 3 * sin(T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 * sin( + T_0) ** 2 - 3 * L ** 4 * a1 * a2 ** 2 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 + 3 * L ** 2 * a1 ** 3 * a2 ** 2 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 2 * a2 ** 3 - 2 * a1 ** 4 * a2 ** 3) / sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin(T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) / (L * sqrt(L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) * sin(T_0)) + (L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) + L * a1 ** 3 * a2 * sin(T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin(T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * ( + -8 * L ** 2 * a1 ** 2 * a2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / (( + L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 3) - 8 * L ** 2 * a1 ** 2 * a2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / (( + L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 3) - 4 * L ** 2 * a2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / (( + L ** 2 - a2 ** 2) ** 2 * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 2 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * ( + -2 * L ** 3 * a1 * sin( + T_0) - 4 * L ** 3 * a2 * sin( + T_0) + 6 * L * a1 * a2 ** 2 * sin( + T_0) - 2 * a2 * ( + L ** 6 * a1 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 * sin( + T_0) ** 2 - L ** 6 * a2 - L ** 4 * a1 ** 3 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 * sin( + T_0) ** 2 - 3 * L ** 4 * a1 * a2 ** 2 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 + 3 * L ** 2 * a1 ** 3 * a2 ** 2 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 2 * a2 ** 3 - 2 * a1 ** 4 * a2 ** 3) / sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4) - 2 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin( + T_0) ** 2 / (( + L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 2 * L ** 2 * ( + -2 * L ** 3 * a1 * sin( + T_0) + 2 * L * a1 ** 3 * sin( + T_0) - 2 * a1 * ( + L ** 6 * a1 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 * sin( + T_0) ** 2 - L ** 6 * a2 - L ** 4 * a1 ** 3 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 * sin( + T_0) ** 2 - 3 * L ** 4 * a1 * a2 ** 2 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 + 3 * L ** 2 * a1 ** 3 * a2 ** 2 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 2 * a2 ** 3 - 2 * a1 ** 4 * a2 ** 3) / sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin( + T_0) ** 2 / (( + L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) / ( + L * sqrt( + L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) * sqrt( + 4 * L ** 2 * sin( + T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / (( + L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / (( + L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * sin( + T_0))) * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) / ( + L * sqrt(L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) * ((4 * L ** 2 * sin( + T_0) ** 2 - 4 * L ** 2 * (L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin(T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * (L ** 5 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 / (L ** 2 * ( + L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2 * sin(T_0) ** 2) + ( + 4 * L ** 2 * sin( + T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / (( + L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / (( + L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 / ( + L ** 2 * ( + L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2 * sin( + T_0) ** 2)) * sin( + T_0)) + sqrt(4 * L ** 2 * sin(T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin(T_0) ** 2 / ( + (L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * (2 * a1 ** 2 * a2 * sqrt( + 4 * L ** 2 * sin(T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin(T_0) ** 2 / ( + (L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin(T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin(T_0) ** 2 / ( + (L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * (L ** 5 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin(T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt(-2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin(T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin(T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) / (L * sqrt(L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2 * sin(T_0)) + a2 * sqrt( + 4 * L ** 2 * sin(T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin(T_0) ** 2 / ( + (L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin(T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin(T_0) ** 2 / ( + (L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * (L ** 5 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin(T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt(-2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin(T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin(T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) / (L * (L ** 2 - a2 ** 2) ** ( + 3 / 2) * (L ** 4 - a1 ** 2 * a2 ** 2) * sin(T_0)) + sqrt( + 4 * L ** 2 * sin(T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin(T_0) ** 2 / ( + (L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin(T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin(T_0) ** 2 / ( + (L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * (-L ** 3 * a1 * sin( + T_0) - 2 * L ** 3 * a2 * sin(T_0) + 3 * L * a1 * a2 ** 2 * sin(T_0) - a2 * (L ** 6 * a1 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 * sin(T_0) ** 2 - L ** 6 * a2 - L ** 4 * a1 ** 3 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 * sin(T_0) ** 2 - 3 * L ** 4 * a1 * a2 ** 2 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 + 3 * L ** 2 * a1 ** 3 * a2 ** 2 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 2 * a2 ** 3 - 2 * a1 ** 4 * a2 ** 3) / sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin(T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4) - sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin(T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) / (L * sqrt(L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) * sin(T_0)) + (L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin(T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin(T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * ( + -8 * L ** 2 * a1 ** 2 * a2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / (( + L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 3) - 8 * L ** 2 * a1 ** 2 * a2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / (( + L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 3) - 4 * L ** 2 * a2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / (( + L ** 2 - a2 ** 2) ** 2 * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 2 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * ( + -2 * L ** 3 * a1 * sin( + T_0) - 4 * L ** 3 * a2 * sin( + T_0) + 6 * L * a1 * a2 ** 2 * sin( + T_0) - 2 * a2 * ( + L ** 6 * a1 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 * sin( + T_0) ** 2 - L ** 6 * a2 - L ** 4 * a1 ** 3 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 * sin( + T_0) ** 2 - 3 * L ** 4 * a1 * a2 ** 2 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 + 3 * L ** 2 * a1 ** 3 * a2 ** 2 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 2 * a2 ** 3 - 2 * a1 ** 4 * a2 ** 3) / sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4) - 2 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin( + T_0) ** 2 / (( + L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 2 * L ** 2 * ( + -2 * L ** 3 * a1 * sin( + T_0) + 2 * L * a1 ** 3 * sin( + T_0) - 2 * a1 * ( + L ** 6 * a1 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 * sin( + T_0) ** 2 - L ** 6 * a2 - L ** 4 * a1 ** 3 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 * sin( + T_0) ** 2 - 3 * L ** 4 * a1 * a2 ** 2 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 + 3 * L ** 2 * a1 ** 3 * a2 ** 2 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 2 * a2 ** 3 - 2 * a1 ** 4 * a2 ** 3) / sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * sin( + T_0) ** 2 / (( + L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) / ( + L * sqrt( + L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) * sqrt( + 4 * L ** 2 * sin( + T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / (( + L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / (( + L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * sin( + T_0))) * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin(T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) / ( + L * sqrt(L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) * ((4 * L ** 2 * sin( + T_0) ** 2 - 4 * L ** 2 * (L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin(T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / ((L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * (L ** 5 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 / (L ** 2 * ( + L ** 2 - a2 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2 * sin(T_0) ** 2) + ( + 4 * L ** 2 * sin( + T_0) ** 2 - 4 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / (( + L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 * sin( + T_0) ** 2 / (( + L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 / ( + L ** 2 * ( + L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2 * sin( + T_0) ** 2)) * sin( + T_0))] + + jac[4] = [(-16 * L ** 2 * a1 * a2 ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin(T_0) - L ** 3 * a2 ** 2 * sin( + T_0) + L * a1 * a2 ** 3 * sin(T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin(T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 / ((L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 3) - 16 * L ** 2 * a1 * a2 ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 / ( + (L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 3) - 8 * L ** 2 * a1 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 / ( + (L ** 2 - a1 ** 2) ** 2 * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + -2 * L ** 3 * a2 * sin(T_0) + 2 * L * a2 ** 3 * sin( + T_0) - 2 * a2 * (2 * L ** 6 * a1 * sin( + T_0) ** 2 - L ** 6 * a1 + L ** 6 * a2 * sin( + T_0) ** 2 - 3 * L ** 4 * a1 ** 2 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 2 * sin( + T_0) ** 2 - L ** 4 * a2 ** 3 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 2 + 3 * L ** 2 * a1 ** 2 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 * a2 ** 4 - 2 * a1 ** 3 * a2 ** 4) / sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin(T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) / ( + (L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin(T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * ( + -4 * L ** 3 * a1 * sin(T_0) - 2 * L ** 3 * a2 * sin( + T_0) + 6 * L * a1 ** 2 * a2 * sin(T_0) - 2 * a1 * ( + 2 * L ** 6 * a1 * sin( + T_0) ** 2 - L ** 6 * a1 + L ** 6 * a2 * sin( + T_0) ** 2 - 3 * L ** 4 * a1 ** 2 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 2 * sin( + T_0) ** 2 - L ** 4 * a2 ** 3 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 2 + 3 * L ** 2 * a1 ** 2 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 * a2 ** 4 - 2 * a1 ** 3 * a2 ** 4) / sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4) - 2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) / ( + (L ** 2 - a1 ** 2) * (L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) / ( + 2 * L ** 2 * sqrt(1 - (2 * L ** 2 - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin(T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 / ( + (L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt(-2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 / ( + (L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) ** 2 / ( + 4 * L ** 4))), + + ( + -16 * L ** 2 * a1 ** 2 * a2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin(T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 / ( + (L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 3) - 16 * L ** 2 * a1 ** 2 * a2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin(T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 / ( + (L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 3) - 8 * L ** 2 * a2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin(T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt(-2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 / ( + (L ** 2 - a2 ** 2) ** 2 * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin(T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt(-2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * ( + -2 * L ** 3 * a1 * sin(T_0) - 4 * L ** 3 * a2 * sin( + T_0) + 6 * L * a1 * a2 ** 2 * sin(T_0) - 2 * a2 * ( + L ** 6 * a1 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 * sin( + T_0) ** 2 - L ** 6 * a2 - L ** 4 * a1 ** 3 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 * sin( + T_0) ** 2 - 3 * L ** 4 * a1 * a2 ** 2 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 + 3 * L ** 2 * a1 ** 3 * a2 ** 2 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 2 * a2 ** 3 - 2 * a1 ** 4 * a2 ** 3) / sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4) - 2 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) / ( + (L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + -2 * L ** 3 * a1 * sin(T_0) + 2 * L * a1 ** 3 * sin( + T_0) - 2 * a1 * (L ** 6 * a1 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 * sin( + T_0) ** 2 - L ** 6 * a2 - L ** 4 * a1 ** 3 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 * sin( + T_0) ** 2 - 3 * L ** 4 * a1 * a2 ** 2 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 + 3 * L ** 2 * a1 ** 3 * a2 ** 2 * sin( + T_0) ** 2 + 2 * L ** 2 * a1 ** 2 * a2 ** 3 - 2 * a1 ** 4 * a2 ** 3) / sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) * ( + L ** 5 * sin(T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin(T_0) - a1 * sqrt( + -2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) / ( + (L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) / ( + 2 * L ** 2 * sqrt(1 - (2 * L ** 2 - 4 * L ** 2 * ( + L ** 5 * sin(T_0) - L ** 3 * a1 * a2 * sin( + T_0) - L ** 3 * a2 ** 2 * sin(T_0) + L * a1 * a2 ** 3 * sin( + T_0) - a2 * sqrt( + -2 * L ** 8 * sin(T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 / ( + (L ** 2 - a2 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2) - 4 * L ** 2 * ( + L ** 5 * sin( + T_0) - L ** 3 * a1 ** 2 * sin( + T_0) - L ** 3 * a1 * a2 * sin( + T_0) + L * a1 ** 3 * a2 * sin( + T_0) - a1 * sqrt(-2 * L ** 8 * sin( + T_0) ** 2 + L ** 8 + 2 * L ** 6 * a1 ** 2 * sin( + T_0) ** 2 - L ** 6 * a1 ** 2 + 2 * L ** 6 * a1 * a2 * sin( + T_0) ** 2 + 2 * L ** 6 * a2 ** 2 * sin( + T_0) ** 2 - L ** 6 * a2 ** 2 - 2 * L ** 4 * a1 ** 3 * a2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 ** 2 * a2 ** 2 * sin( + T_0) ** 2 - 2 * L ** 4 * a1 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 4 * a2 ** 2 + 2 * L ** 2 * a1 ** 3 * a2 ** 3 * sin( + T_0) ** 2 + L ** 2 * a1 ** 2 * a2 ** 4 - a1 ** 4 * a2 ** 4)) ** 2 / ( + (L ** 2 - a1 ** 2) * ( + L ** 4 - a1 ** 2 * a2 ** 2) ** 2)) ** 2 / ( + 4 * L ** 4)))] + + return jac + + + + +def jacobian_euler(a1, a2, L, T_0): + """ + From: + https://colab.research.google.com/drive/11faUc8pS1yWxFrnmt05VqpDsqOwEi_dg#scrollTo=xx7_60I1sV9j&line=1&uniqifier=1 + """ + t = CommonTerms(L=L, T_0=T_0, a1=a1, a2=a2) + + jac = np.zeros((6, 2), dtype=float) + jac[0, 0] = (4 * L * a1 * t.a2_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_03) * t.sin_t0 / ( + sqrt(t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) + 2 * L * ( + -t.l_p3 * a2 * t.sin_t0 + L * t.a2_p3 * t.sin_t0 - a2 * ( + 2 * t.l_p6 * a1 * t.sin_t0_sq - t.l_p6 * a1 + t.l_p6 * a2 * t.sin_t0_sq - 3 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq - t.l_p4 * t.a2_p3 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p3 * t.a2_p2 + 3 * t.l_p2 * t.a1_p2 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * a1 * t.a2_p4 - 2 * t.a1_p3 * t.a2_p4) / t.sqrt_05) * t.sin_t0 / ( + sqrt(t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq)) + jac[0, 1] = (4 * L * t.a1_p2 * a2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_03) * t.sin_t0 / ( + sqrt(t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) + 2 * L * a2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_05) * t.sin_t0 / ( + (t.l_p2 - t.a2_p2) ** (3 / 2) * t.l_p4_minus_a1_sq_mul_a2_sq) + 2 * L * ( + -t.l_p3 * a1 * t.sin_t0 - 2 * t.l_p3 * a2 * t.sin_t0 + 3 * L * a1 * t.a2_p2 * t.sin_t0 - a2 * ( + t.l_p6 * a1 * t.sin_t0_sq + 2 * t.l_p6 * a2 * t.sin_t0_sq - t.l_p6 * a2 - t.l_p4 * t.a1_p3 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 3 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * a2 + 3 * t.l_p2 * t.a1_p3 * t.a2_p2 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p2 * t.a2_p3 - 2 * t.a1_p4 * t.a2_p3) / t.sqrt_05 - t.sqrt_05) * t.sin_t0 / ( + sqrt(t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq)) + jac[1, 0] = (4 * L * a1 * t.a2_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_03) * t.sin_t0 / ( + sqrt(t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) + 2 * L * a1 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_05) * t.sin_t0 / ( + (t.l_p2 - t.a1_p2) ** (3 / 2) * t.l_p4_minus_a1_sq_mul_a2_sq) + 2 * L * ( + -2 * t.l_p3 * a1 * t.sin_t0 - t.l_p3 * a2 * t.sin_t0 + 3 * L * t.a1_p2 * a2 * t.sin_t0 - a1 * ( + 2 * t.l_p6 * a1 * t.sin_t0_sq - t.l_p6 * a1 + t.l_p6 * a2 * t.sin_t0_sq - 3 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq - t.l_p4 * t.a2_p3 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p3 * t.a2_p2 + 3 * t.l_p2 * t.a1_p2 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * a1 * t.a2_p4 - 2 * t.a1_p3 * t.a2_p4) / t.sqrt_05 - t.sqrt_05) * t.sin_t0 / ( + sqrt(t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq)) + jac[1, 1] = (4 * L * t.a1_p2 * a2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_03) * t.sin_t0 / ( + sqrt(t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) + 2 * L * ( + -t.l_p3 * a1 * t.sin_t0 + L * t.a1_p3 * t.sin_t0 - a1 * ( + t.l_p6 * a1 * t.sin_t0_sq + 2 * t.l_p6 * a2 * t.sin_t0_sq - t.l_p6 * a2 - t.l_p4 * t.a1_p3 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 3 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * a2 + 3 * t.l_p2 * t.a1_p3 * t.a2_p2 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p2 * t.a2_p3 - 2 * t.a1_p4 * t.a2_p3) / t.sqrt_05) * t.sin_t0 / ( + sqrt(t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq)) + + jac[2, 0] = (4 * L * a1 * t.a2_p2 * ( + L * t.l_p4_minus_a1_sq_mul_a2_sq * (a1 + a2) * t.sin_t0 + (t.l_p2 + a1 * a2) * sqrt( + t.l_p2 * (t.l_p2 * a1 + t.l_p2 * a2 - t.a1_p2 * a2 - a1 * t.a2_p2) ** 2 * t.sin_t0_sq + ( + -t.l_p4 + t.a1_p2 * t.a2_p2) * ( + -2 * t.l_p4 * t.cos_t0_sq + t.l_p4 + t.l_p2 * t.a1_p2 * t.cos_t0_sq + t.l_p2 * t.a2_p2 * t.cos_t0_sq - t.a1_p2 * t.a2_p2))) * t.sin_t0 / ( + (t.l_p2 + a1 * a2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 2 * L * a2 * ( + L * t.l_p4_minus_a1_sq_mul_a2_sq * (a1 + a2) * t.sin_t0 + (t.l_p2 + a1 * a2) * sqrt( + t.l_p2 * (t.l_p2 * a1 + t.l_p2 * a2 - t.a1_p2 * a2 - a1 * t.a2_p2) ** 2 * t.sin_t0_sq + ( + -t.l_p4 + t.a1_p2 * t.a2_p2) * ( + -2 * t.l_p4 * t.cos_t0_sq + t.l_p4 + t.l_p2 * t.a1_p2 * t.cos_t0_sq + t.l_p2 * t.a2_p2 * t.cos_t0_sq - t.a1_p2 * t.a2_p2))) * t.sin_t0 / ( + (t.l_p2 + a1 * a2) ** 2 * t.l_p4_minus_a1_sq_mul_a2_sq) + 2 * L * ( + -2 * L * a1 * t.a2_p2 * (a1 + a2) * t.sin_t0 + L * ( + t.l_p4 - t.a1_p2 * t.a2_p2) * t.sin_t0 + a2 * sqrt( + t.l_p2 * (t.l_p2 * a1 + t.l_p2 * a2 - t.a1_p2 * a2 - a1 * t.a2_p2) ** 2 * t.sin_t0_sq + ( + -t.l_p4 + t.a1_p2 * t.a2_p2) * ( + -2 * t.l_p4 * t.cos_t0_sq + t.l_p4 + t.l_p2 * t.a1_p2 * t.cos_t0_sq + t.l_p2 * t.a2_p2 * t.cos_t0_sq - t.a1_p2 * t.a2_p2)) + ( + t.l_p2 + a1 * a2) * (t.l_p2 * (2 * t.l_p2 - 4 * a1 * a2 - 2 * t.a2_p2) * ( + t.l_p2 * a1 + t.l_p2 * a2 - t.a1_p2 * a2 - a1 * t.a2_p2) * t.sin_t0_sq / 2 + a1 * t.a2_p2 * ( + -2 * t.l_p4 * t.cos_t0_sq + t.l_p4 + t.l_p2 * t.a1_p2 * t.cos_t0_sq + t.l_p2 * t.a2_p2 * t.cos_t0_sq - t.a1_p2 * t.a2_p2) + ( + -t.l_p4 + t.a1_p2 * t.a2_p2) * ( + 2 * t.l_p2 * a1 * t.cos_t0_sq - 2 * a1 * t.a2_p2) / 2) / sqrt( + t.l_p2 * (t.l_p2 * a1 + t.l_p2 * a2 - t.a1_p2 * a2 - a1 * t.a2_p2) ** 2 * t.sin_t0_sq + ( + -t.l_p4 + t.a1_p2 * t.a2_p2) * ( + -2 * t.l_p4 * t.cos_t0_sq + t.l_p4 + t.l_p2 * t.a1_p2 * t.cos_t0_sq + t.l_p2 * t.a2_p2 * t.cos_t0_sq - t.a1_p2 * t.a2_p2))) * t.sin_t0 / ( + (t.l_p2 + a1 * a2) * t.l_p4_minus_a1_sq_mul_a2_sq)) + + jac[2, 1] = (4 * L * t.a1_p2 * a2 * ( + L * t.l_p4_minus_a1_sq_mul_a2_sq * (a1 + a2) * t.sin_t0 + (t.l_p2 + a1 * a2) * sqrt( + t.l_p2 * (t.l_p2 * a1 + t.l_p2 * a2 - t.a1_p2 * a2 - a1 * t.a2_p2) ** 2 * t.sin_t0_sq + ( + -t.l_p4 + t.a1_p2 * t.a2_p2) * ( + -2 * t.l_p4 * t.cos_t0_sq + t.l_p4 + t.l_p2 * t.a1_p2 * t.cos_t0_sq + t.l_p2 * t.a2_p2 * t.cos_t0_sq - t.a1_p2 * t.a2_p2))) * t.sin_t0 / ( + (t.l_p2 + a1 * a2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 2 * L * a1 * ( + L * t.l_p4_minus_a1_sq_mul_a2_sq * (a1 + a2) * t.sin_t0 + (t.l_p2 + a1 * a2) * sqrt( + t.l_p2 * (t.l_p2 * a1 + t.l_p2 * a2 - t.a1_p2 * a2 - a1 * t.a2_p2) ** 2 * t.sin_t0_sq + ( + -t.l_p4 + t.a1_p2 * t.a2_p2) * ( + -2 * t.l_p4 * t.cos_t0_sq + t.l_p4 + t.l_p2 * t.a1_p2 * t.cos_t0_sq + t.l_p2 * t.a2_p2 * t.cos_t0_sq - t.a1_p2 * t.a2_p2))) * t.sin_t0 / ( + (t.l_p2 + a1 * a2) ** 2 * t.l_p4_minus_a1_sq_mul_a2_sq) + 2 * L * ( + -2 * L * t.a1_p2 * a2 * (a1 + a2) * t.sin_t0 + L * ( + t.l_p4 - t.a1_p2 * t.a2_p2) * t.sin_t0 + a1 * sqrt( + t.l_p2 * (t.l_p2 * a1 + t.l_p2 * a2 - t.a1_p2 * a2 - a1 * t.a2_p2) ** 2 * t.sin_t0_sq + ( + -t.l_p4 + t.a1_p2 * t.a2_p2) * ( + -2 * t.l_p4 * t.cos_t0_sq + t.l_p4 + t.l_p2 * t.a1_p2 * t.cos_t0_sq + t.l_p2 * t.a2_p2 * t.cos_t0_sq - t.a1_p2 * t.a2_p2)) + ( + t.l_p2 + a1 * a2) * (t.l_p2 * (2 * t.l_p2 - 2 * t.a1_p2 - 4 * a1 * a2) * ( + t.l_p2 * a1 + t.l_p2 * a2 - t.a1_p2 * a2 - a1 * t.a2_p2) * t.sin_t0_sq / 2 + t.a1_p2 * a2 * ( + -2 * t.l_p4 * t.cos_t0_sq + t.l_p4 + t.l_p2 * t.a1_p2 * t.cos_t0_sq + t.l_p2 * t.a2_p2 * t.cos_t0_sq - t.a1_p2 * t.a2_p2) + ( + -t.l_p4 + t.a1_p2 * t.a2_p2) * ( + 2 * t.l_p2 * a2 * t.cos_t0_sq - 2 * t.a1_p2 * a2) / 2) / sqrt( + t.l_p2 * (t.l_p2 * a1 + t.l_p2 * a2 - t.a1_p2 * a2 - a1 * t.a2_p2) ** 2 * t.sin_t0_sq + ( + -t.l_p4 + t.a1_p2 * t.a2_p2) * ( + -2 * t.l_p4 * t.cos_t0_sq + t.l_p4 + t.l_p2 * t.a1_p2 * t.cos_t0_sq + t.l_p2 * t.a2_p2 * t.cos_t0_sq - t.a1_p2 * t.a2_p2))) * t.sin_t0 / ( + (t.l_p2 + a1 * a2) * t.l_p4_minus_a1_sq_mul_a2_sq)) + + + + + jac[3, 0] = (-(2 * t.l_p2 - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_03) ** 2 / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * (2 * a1 * t.a2_p2 * sqrt( + 4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.cos_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_02) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.cos_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_02) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.cos_t0 - t.l_p3 * a1 * a2 * t.cos_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_04) / ( + L * sqrt( + t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2 * t.sin_t0) + a1 * sqrt( + 4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.cos_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_02) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.cos_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_02) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.cos_t0 - t.l_p3 * a1 * a2 * t.cos_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_04) / ( + L * ( + t.l_p2 - t.a1_p2) ** ( + 3 / 2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) * t.sin_t0) + sqrt( + 4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.cos_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_02) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.cos_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_02) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * ( + -2 * t.l_p3 * a1 * t.sin_t0 - t.l_p3 * a2 * t.cos_t0 + 3 * L * t.a1_p2 * a2 * t.cos_t0 - a1 * ( + 2 * t.l_p6 * a1 * t.sin_t0_sq - t.l_p6 * a1 + t.l_p6 * a2 * t.sin_t0_sq - 3 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq - t.l_p4 * t.a2_p3 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p3 * t.a2_p2 + 3 * t.l_p2 * t.a1_p2 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * a1 * t.a2_p4 - 2 * t.a1_p3 * t.a2_p4) / t.sqrt_04 - t.sqrt_04) / ( + L * sqrt( + t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) * t.sin_t0) + ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.cos_t0 - t.l_p3 * a1 * a2 * t.cos_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_04) * ( + -8 * t.l_p2 * a1 * t.a2_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_04) ** 2 * t.sin_t0_sq / ( + ( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 3) - 8 * t.l_p2 * a1 * t.a2_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + ( + t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 3) - 4 * t.l_p2 * a1 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + ( + t.l_p2 - t.a1_p2) ** 2 * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 2 * t.l_p2 * ( + -2 * t.l_p3 * a2 * t.sin_t0 + 2 * L * t.a2_p3 * t.sin_t0 - 2 * a2 * ( + 2 * t.l_p6 * a1 * t.sin_t0_sq - t.l_p6 * a1 + t.l_p6 * a2 * t.sin_t0_sq - 3 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq - t.l_p4 * t.a2_p3 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p3 * t.a2_p2 + 3 * t.l_p2 * t.a1_p2 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * a1 * t.a2_p4 - 2 * t.a1_p3 * t.a2_p4) / sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * t.sin_t0_sq / ( + ( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 2 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * ( + -4 * t.l_p3 * a1 * t.sin_t0 - 2 * t.l_p3 * a2 * t.sin_t0 + 6 * L * t.a1_p2 * a2 * t.sin_t0 - 2 * a1 * ( + 2 * t.l_p6 * a1 * t.sin_t0_sq - t.l_p6 * a1 + t.l_p6 * a2 * t.sin_t0_sq - 3 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq - t.l_p4 * t.a2_p3 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p3 * t.a2_p2 + 3 * t.l_p2 * t.a1_p2 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * a1 * t.a2_p4 - 2 * t.a1_p3 * t.a2_p4) / sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4) - 2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * t.sin_t0_sq / ( + ( + t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2)) / ( + L * sqrt( + t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) * sqrt( + 4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + ( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + ( + t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2)) * t.sin_t0)) / ( + 2 * t.l_p2 * ((4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_05) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + t.l_p2 * (t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2 * t.sin_t0_sq) + ( + 2 * t.l_p2 - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + (t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + (t.l_p2 - t.a1_p2) * (t.l_p4 - t.a1_p2 * t.a2_p2) ** 2)) ** 2 / ( + 4 * t.l_p4))) + sqrt(4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_03) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_05) * ( + -16 * t.l_p2 * a1 * t.a2_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_05) ** 2 / ( + (t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 3) - 16 * t.l_p2 * a1 * t.a2_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_01) ** 2 / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 3) - 8 * t.l_p2 * a1 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_01) ** 2 / ( + (t.l_p2 - t.a1_p2) ** 2 * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + -2 * t.l_p3 * a2 * t.sin_t0 + 2 * L * t.a2_p3 * t.sin_t0 - 2 * a2 * ( + 2 * t.l_p6 * a1 * t.sin_t0_sq - t.l_p6 * a1 + t.l_p6 * a2 * t.sin_t0_sq - 3 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq - t.l_p4 * t.a2_p3 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p3 * t.a2_p2 + 3 * t.l_p2 * t.a1_p2 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * a1 * t.a2_p4 - 2 * t.a1_p3 * t.a2_p4) / t.sqrt_01) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_01) / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_01) * ( + -4 * t.l_p3 * a1 * t.sin_t0 - 2 * t.l_p3 * a2 * t.sin_t0 + 6 * L * t.a1_p2 * a2 * t.sin_t0 - 2 * a1 * ( + 2 * t.l_p6 * a1 * t.sin_t0_sq - t.l_p6 * a1 + t.l_p6 * a2 * t.sin_t0_sq - 3 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq - t.l_p4 * t.a2_p3 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p3 * t.a2_p2 + 3 * t.l_p2 * t.a1_p2 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * a1 * t.a2_p4 - 2 * t.a1_p3 * t.a2_p4) / t.sqrt_01 - 2 * t.sqrt_01) / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) / ( + 2 * t.l_p3 * sqrt(t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq * (( + 4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + ( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + ( + t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + t.l_p2 * ( + t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2 * t.sin_t0_sq) + ( + 2 * t.l_p2 - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + ( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + ( + t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2)) ** 2 / ( + 4 * t.l_p4)) * t.sin_t0)) + + + + + + jac[3, 1] = (-(2 * t.l_p2 - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_03) ** 2 / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * (2 * t.a1_p2 * a2 * sqrt( + 4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_02) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_02) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_04) / ( + L * sqrt( + t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2 * t.sin_t0) + sqrt( + 4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_02) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_02) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * ( + -t.l_p3 * a1 * t.sin_t0 + L * t.a1_p3 * t.sin_t0 - a1 * ( + t.l_p6 * a1 * t.sin_t0_sq + 2 * t.l_p6 * a2 * t.sin_t0_sq - t.l_p6 * a2 - t.l_p4 * t.a1_p3 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 3 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * a2 + 3 * t.l_p2 * t.a1_p3 * t.a2_p2 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p2 * t.a2_p3 - 2 * t.a1_p4 * t.a2_p3) / t.sqrt_04) / ( + L * sqrt( + t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) * t.sin_t0) + ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_04) * ( + -8 * t.l_p2 * t.a1_p2 * a2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_04) ** 2 * t.sin_t0_sq / ( + ( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 3) - 8 * t.l_p2 * t.a1_p2 * a2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + ( + t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 3) - 4 * t.l_p2 * a2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + ( + t.l_p2 - t.a2_p2) ** 2 * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 2 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * ( + -2 * t.l_p3 * a1 * t.sin_t0 - 4 * t.l_p3 * a2 * t.sin_t0 + 6 * L * a1 * t.a2_p2 * t.sin_t0 - 2 * a2 * ( + t.l_p6 * a1 * t.sin_t0_sq + 2 * t.l_p6 * a2 * t.sin_t0_sq - t.l_p6 * a2 - t.l_p4 * t.a1_p3 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 3 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * a2 + 3 * t.l_p2 * t.a1_p3 * t.a2_p2 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p2 * t.a2_p3 - 2 * t.a1_p4 * t.a2_p3) / sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4) - 2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * t.sin_t0_sq / ( + ( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 2 * t.l_p2 * ( + -2 * t.l_p3 * a1 * t.sin_t0 + 2 * L * t.a1_p3 * t.sin_t0 - 2 * a1 * ( + t.l_p6 * a1 * t.sin_t0_sq + 2 * t.l_p6 * a2 * t.sin_t0_sq - t.l_p6 * a2 - t.l_p4 * t.a1_p3 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 3 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * a2 + 3 * t.l_p2 * t.a1_p3 * t.a2_p2 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p2 * t.a2_p3 - 2 * t.a1_p4 * t.a2_p3) / sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * t.sin_t0_sq / ( + ( + t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2)) / ( + L * sqrt( + t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) * sqrt( + 4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + ( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + ( + t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2)) * t.sin_t0)) / ( + 2 * t.l_p2 * ((4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.cos_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_05) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.cos_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.cos_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.cos_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.cos_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + t.l_p2 * (t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2 * t.sin_t0_sq) + ( + 2 * t.l_p2 - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.cos_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + (t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + (t.l_p2 - t.a1_p2) * (t.l_p4 - t.a1_p2 * t.a2_p2) ** 2)) ** 2 / ( + 4 * t.l_p4))) + sqrt(4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_03) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_05) * ( + -16 * t.l_p2 * t.a1_p2 * a2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_05) ** 2 / ( + (t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 3) - 16 * t.l_p2 * t.a1_p2 * a2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_01) ** 2 / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 3) - 8 * t.l_p2 * a2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_01) ** 2 / ( + (t.l_p2 - t.a2_p2) ** 2 * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_01) * ( + -2 * t.l_p3 * a1 * t.sin_t0 - 4 * t.l_p3 * a2 * t.sin_t0 + 6 * L * a1 * t.a2_p2 * t.sin_t0 - 2 * a2 * ( + t.l_p6 * a1 * t.sin_t0_sq + 2 * t.l_p6 * a2 * t.sin_t0_sq - t.l_p6 * a2 - t.l_p4 * t.a1_p3 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 3 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * a2 + 3 * t.l_p2 * t.a1_p3 * t.a2_p2 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p2 * t.a2_p3 - 2 * t.a1_p4 * t.a2_p3) / t.sqrt_01 - 2 * t.sqrt_01) / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + -2 * t.l_p3 * a1 * t.sin_t0 + 2 * L * t.a1_p3 * t.sin_t0 - 2 * a1 * ( + t.l_p6 * a1 * t.sin_t0_sq + 2 * t.l_p6 * a2 * t.sin_t0_sq - t.l_p6 * a2 - t.l_p4 * t.a1_p3 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 3 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * a2 + 3 * t.l_p2 * t.a1_p3 * t.a2_p2 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p2 * t.a2_p3 - 2 * t.a1_p4 * t.a2_p3) / t.sqrt_01) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_01) / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) / ( + 2 * t.l_p3 * sqrt(t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq * (( + 4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + ( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + ( + t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + t.l_p2 * ( + t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2 * t.sin_t0_sq) + ( + 2 * t.l_p2 - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + ( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + ( + t.l_p2 - t.a1_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2)) ** 2 / ( + 4 * t.l_p4)) * t.sin_t0) + + ) + jac[4, 0] = (-(2 * a1 * t.a2_p2 * sqrt(4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_03) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) / ( + L * sqrt(t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2 * t.sin_t0) + sqrt( + 4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_02) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_02) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * ( + -t.l_p3 * a2 * t.sin_t0 + L * t.a2_p3 * t.sin_t0 - a2 * ( + 2 * t.l_p6 * a1 * t.sin_t0_sq - t.l_p6 * a1 + t.l_p6 * a2 * t.sin_t0_sq - 3 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq - t.l_p4 * t.a2_p3 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p3 * t.a2_p2 + 3 * t.l_p2 * t.a1_p2 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * a1 * t.a2_p4 - 2 * t.a1_p3 * t.a2_p4) / sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) / ( + L * sqrt(t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq * t.sin_t0) + ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * ( + -8 * t.l_p2 * a1 * t.a2_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 3) - 8 * t.l_p2 * a1 * t.a2_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 3) - 4 * t.l_p2 * a1 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) ** 2 * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 2 * t.l_p2 * ( + -2 * t.l_p3 * a2 * t.sin_t0 + 2 * L * t.a2_p3 * t.sin_t0 - 2 * a2 * ( + 2 * t.l_p6 * a1 * t.sin_t0_sq - t.l_p6 * a1 + t.l_p6 * a2 * t.sin_t0_sq - 3 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq - t.l_p4 * t.a2_p3 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p3 * t.a2_p2 + 3 * t.l_p2 * t.a1_p2 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * a1 * t.a2_p4 - 2 * t.a1_p3 * t.a2_p4) / sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 2 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * ( + -4 * t.l_p3 * a1 * t.sin_t0 - 2 * t.l_p3 * a2 * t.sin_t0 + 6 * L * t.a1_p2 * a2 * t.sin_t0 - 2 * a1 * ( + 2 * t.l_p6 * a1 * t.sin_t0_sq - t.l_p6 * a1 + t.l_p6 * a2 * t.sin_t0_sq - 3 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq - t.l_p4 * t.a2_p3 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p3 * t.a2_p2 + 3 * t.l_p2 * t.a1_p2 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * a1 * t.a2_p4 - 2 * t.a1_p3 * t.a2_p4) / sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4) - 2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) / ( + L * sqrt(t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq * sqrt( + 4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * t.sin_t0)) / sqrt(1 - ( + 4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_03) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.cos_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + t.l_p2 * ( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2 * t.sin_t0_sq))) + jac[4, 1] = (-(2 * t.a1_p2 * a2 * sqrt(4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_03) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) / ( + L * sqrt(t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2 * t.sin_t0) + a2 * sqrt( + 4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_02) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.cos_t0 - a1 * t.sqrt_02) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) / ( + L * (t.l_p2 - t.a2_p2) ** (3 / 2) * t.l_p4_minus_a1_sq_mul_a2_sq * t.sin_t0) + sqrt( + 4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_02) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_02) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * ( + -t.l_p3 * a1 * t.sin_t0 - 2 * t.l_p3 * a2 * t.sin_t0 + 3 * L * a1 * t.a2_p2 * t.sin_t0 - a2 * ( + t.l_p6 * a1 * t.sin_t0_sq + 2 * t.l_p6 * a2 * t.sin_t0_sq - t.l_p6 * a2 - t.l_p4 * t.a1_p3 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 3 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * a2 + 3 * t.l_p2 * t.a1_p3 * t.a2_p2 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p2 * t.a2_p3 - 2 * t.a1_p4 * t.a2_p3) / sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4) - sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) / ( + L * sqrt(t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq * t.sin_t0) + ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * ( + -8 * t.l_p2 * t.a1_p2 * a2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 3) - 8 * t.l_p2 * t.a1_p2 * a2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 3) - 4 * t.l_p2 * a2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) ** 2 * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 2 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * ( + -2 * t.l_p3 * a1 * t.sin_t0 - 4 * t.l_p3 * a2 * t.sin_t0 + 6 * L * a1 * t.a2_p2 * t.sin_t0 - 2 * a2 * ( + t.l_p6 * a1 * t.sin_t0_sq + 2 * t.l_p6 * a2 * t.sin_t0_sq - t.l_p6 * a2 - t.l_p4 * t.a1_p3 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 3 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * a2 + 3 * t.l_p2 * t.a1_p3 * t.a2_p2 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p2 * t.a2_p3 - 2 * t.a1_p4 * t.a2_p3) / sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4) - 2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 2 * t.l_p2 * ( + -2 * t.l_p3 * a1 * t.sin_t0 + 2 * L * t.a1_p3 * t.sin_t0 - 2 * a1 * ( + t.l_p6 * a1 * t.sin_t0_sq + 2 * t.l_p6 * a2 * t.sin_t0_sq - t.l_p6 * a2 - t.l_p4 * t.a1_p3 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 3 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * a2 + 3 * t.l_p2 * t.a1_p3 * t.a2_p2 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p2 * t.a2_p3 - 2 * t.a1_p4 * t.a2_p3) / sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) / ( + L * sqrt(t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq * sqrt( + 4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * t.sin_t0)) / sqrt(1 - ( + 4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_03) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.cos_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * t.sin_t0_sq / ( + (t.l_p2 - t.a1_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + t.l_p2 * ( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2 * t.sin_t0_sq)) + + ) + jac[5, 0] = ((1 - 2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.cos_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_03) ** 2 / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * (-8 * a1 * t.a2_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.cos_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_03) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) / ( + sqrt( + t.l_p2 - t.a1_p2) * sqrt( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 3) - 2 * a1 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) / ( + (t.l_p2 - t.a1_p2) ** ( + 3 / 2) * sqrt( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 2 * ( + -t.l_p3 * a2 * t.sin_t0 + L * t.a2_p3 * t.sin_t0 - a2 * ( + 2 * t.l_p6 * a1 * t.sin_t0_sq - t.l_p6 * a1 + t.l_p6 * a2 * t.sin_t0_sq - 3 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq - t.l_p4 * t.a2_p3 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p3 * t.a2_p2 + 3 * t.l_p2 * t.a1_p2 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * a1 * t.a2_p4 - 2 * t.a1_p3 * t.a2_p4) / sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) / ( + sqrt( + t.l_p2 - t.a1_p2) * sqrt( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * ( + -2 * t.l_p3 * a1 * t.sin_t0 - t.l_p3 * a2 * t.sin_t0 + 3 * L * t.a1_p2 * a2 * t.sin_t0 - a1 * ( + 2 * t.l_p6 * a1 * t.sin_t0_sq - t.l_p6 * a1 + t.l_p6 * a2 * t.sin_t0_sq - 3 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq - t.l_p4 * t.a2_p3 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p3 * t.a2_p2 + 3 * t.l_p2 * t.a1_p2 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * a1 * t.a2_p4 - 2 * t.a1_p3 * t.a2_p4) / sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4) - sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) / ( + sqrt( + t.l_p2 - t.a1_p2) * sqrt( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2)) / ( + (1 - 2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.cos_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_01) ** 2 / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) ** 2 + 4 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.cos_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_01) ** 2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.cos_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_01) ** 2 / ( + (t.l_p2 - t.a1_p2) * (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 4)) + 2 * ( + -8 * a1 * t.a2_p2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.cos_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_05) ** 2 / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 3) - 2 * ( + -2 * t.l_p3 * a2 * t.sin_t0 + 2 * L * t.a2_p3 * t.sin_t0 - 2 * a2 * ( + 2 * t.l_p6 * a1 * t.sin_t0_sq - t.l_p6 * a1 + t.l_p6 * a2 * t.sin_t0_sq - 3 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq - t.l_p4 * t.a2_p3 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p3 * t.a2_p2 + 3 * t.l_p2 * t.a1_p2 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * a1 * t.a2_p4 - 2 * t.a1_p3 * t.a2_p4) / t.sqrt_01) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.cos_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_01) / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.cos_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_05) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.cos_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_05) / ( + sqrt(t.l_p2 - t.a1_p2) * sqrt(t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2 * ( + (1 - 2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + ( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2)) ** 2 + 4 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + ( + t.l_p2 - t.a1_p2) * ( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 4)))) + + jac[5, 1] = (1 - 2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.cos_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * t.sqrt_03) ** 2 / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * (-8 * t.a1_p2 * a2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.cos_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * t.sqrt_03) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) / ( + sqrt( + t.l_p2 - t.a1_p2) * sqrt( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 3) - 2 * a2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) / ( + sqrt(t.l_p2 - t.a1_p2) * ( + t.l_p2 - t.a2_p2) ** ( + 3 / 2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 2 * ( + -t.l_p3 * a1 * t.sin_t0 + L * t.a1_p3 * t.sin_t0 - a1 * ( + t.l_p6 * a1 * t.sin_t0_sq + 2 * t.l_p6 * a2 * t.sin_t0_sq - t.l_p6 * a2 - t.l_p4 * t.a1_p3 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 3 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * a2 + 3 * t.l_p2 * t.a1_p3 * t.a2_p2 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p2 * t.a2_p3 - 2 * t.a1_p4 * t.a2_p3) / sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) / ( + sqrt( + t.l_p2 - t.a1_p2) * sqrt( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2) - 2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * ( + -t.l_p3 * a1 * t.sin_t0 - 2 * t.l_p3 * a2 * t.sin_t0 + 3 * L * a1 * t.a2_p2 * t.sin_t0 - a2 * ( + t.l_p6 * a1 * t.sin_t0_sq + 2 * t.l_p6 * a2 * t.sin_t0_sq - t.l_p6 * a2 - t.l_p4 * t.a1_p3 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 3 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * a2 + 3 * t.l_p2 * t.a1_p3 * t.a2_p2 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p2 * t.a2_p3 - 2 * t.a1_p4 * t.a2_p3) / sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4) - sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) / ( + sqrt( + t.l_p2 - t.a1_p2) * sqrt( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2)) / ( + (1 - 2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.cos_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) ** 2 + 4 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.cos_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.cos_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + (t.l_p2 - t.a1_p2) * (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 4)) + 2 * ( + -8 * t.a1_p2 * a2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.cos_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 3) - 4 * a2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.cos_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + (t.l_p2 - t.a2_p2) ** 2 * t.l_p4_minus_a1_sq_mul_a2_sq ** 2) - 2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.cos_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * ( + -2 * t.l_p3 * a1 * t.sin_t0 - 4 * t.l_p3 * a2 * t.cos_t0 + 6 * L * a1 * t.a2_p2 * t.sin_t0 - 2 * a2 * ( + t.l_p6 * a1 * t.sin_t0_sq + 2 * t.l_p6 * a2 * t.sin_t0_sq - t.l_p6 * a2 - t.l_p4 * t.a1_p3 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * a2 * t.sin_t0_sq - 3 * t.l_p4 * a1 * t.a2_p2 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * a2 + 3 * t.l_p2 * t.a1_p3 * t.a2_p2 * t.sin_t0_sq + 2 * t.l_p2 * t.a1_p2 * t.a2_p3 - 2 * t.a1_p4 * t.a2_p3) / sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4) - 2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) / ( + (t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.cos_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.cos_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) / ( + sqrt(t.l_p2 - t.a1_p2) * sqrt(t.l_p2 - t.a2_p2) * t.l_p4_minus_a1_sq_mul_a2_sq ** 2 * ( + (1 - 2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + ( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 2)) ** 2 + 4 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * t.a1_p2 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 + L * t.a1_p3 * a2 * t.sin_t0 - a1 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 * ( + t.l_p5 * t.sin_t0 - t.l_p3 * a1 * a2 * t.sin_t0 - t.l_p3 * t.a2_p2 * t.sin_t0 + L * a1 * t.a2_p3 * t.sin_t0 - a2 * sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4)) ** 2 / ( + ( + t.l_p2 - t.a1_p2) * ( + t.l_p2 - t.a2_p2) * ( + t.l_p4 - t.a1_p2 * t.a2_p2) ** 4))) + return np.array(jac) + + + +def f_zenith(a1, a2, L, T_0): + """ + From: https://colab.research.google.com/drive/1Y7hfS1aD7iahi77h3xDdz3hMdDL553Dm#scrollTo=MXNYwnZwK2Xh&line=1&uniqifier=1 + """ + t = CommonTerms(L=L, T_0=T_0, a1=a1, a2=a2) + + return -arcsin((2 * t.l_p2 - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l ** 3 - t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l - t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p5 * t.sin_t0 - L * sqrt( + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l ** 3 - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l_p2 - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l ** 3 + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a2_div_l_p2 - 2 * t.l_p8 * t.sin_t0_sq - t.l_p8 * t.sin_t0_plus_asin_a1_div_l ** 4 * t.sin_t0_plus_asin_a2_div_l ** 4 + t.l_p8 * t.sin_t0_plus_asin_a1_div_l ** 4 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p8 * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l ** 4 - t.l_p8 * t.sin_t0_plus_asin_a1_div_l_p2 - t.l_p8 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p8) * t.sin_t0_plus_asin_a2_div_l) ** 2 / ( + (-t.l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p2) * ( + -t.l_p4 * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p4) ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l - t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l_p2 - t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l + t.l_p5 * t.cos_t0 - L * sqrt( + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l ** 3 - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l_p2 - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l ** 3 + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a2_div_l_p2 - 2 * t.l_p8 * t.sin_t0_sq - t.l_p8 * t.sin_t0_plus_asin_a1_div_l ** 4 * t.sin_t0_plus_asin_a2_div_l ** 4 + t.l_p8 * t.sin_t0_plus_asin_a1_div_l ** 4 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p8 * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l ** 4 - t.l_p8 * t.sin_t0_plus_asin_a1_div_l_p2 - t.l_p8 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p8) * t.sin_t0_plus_asin_a1_div_l) ** 2 / ( + (-t.l_p2 * t.sin_t0_plus_asin_a1_div_l_p2 + t.l_p2) * ( + -t.l_p4 * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p4) ** 2)) / ( + 2 * t.l_p2)) + pi / 2 + + +def f_azimuth(a1, a2, L, T_0): + """ + From: https://colab.research.google.com/drive/1Y7hfS1aD7iahi77h3xDdz3hMdDL553Dm#scrollTo=MXNYwnZwK2Xh&line=1&uniqifier=1 + """ + t = CommonTerms(L=L, T_0=T_0, a1=a1, a2=a2) + + return arctan2(sqrt(4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l ** 3 - t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l - t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p5 * t.cos_t0 - L * sqrt( + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l ** 3 - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l_p2 - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l ** 3 + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a2_div_l_p2 - 2 * t.l_p8 * t.sin_t0_sq - t.l_p8 * t.sin_t0_plus_asin_a1_div_l ** 4 * t.sin_t0_plus_asin_a2_div_l ** 4 + t.l_p8 * t.sin_t0_plus_asin_a1_div_l ** 4 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p8 * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l ** 4 - t.l_p8 * t.sin_t0_plus_asin_a1_div_l_p2 - t.l_p8 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p8) * t.sin_t0_plus_asin_a2_div_l) ** 2 * t.sin_t0_sq / ( + (-t.l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p2) * ( + -t.l_p4 * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p4) ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l - t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l_p2 - t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l + t.l_p5 * t.cos_t0 - L * sqrt( + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l ** 3 - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l_p2 - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l ** 3 + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a2_div_l_p2 - 2 * t.l_p8 * t.sin_t0_sq - t.l_p8 * t.sin_t0_plus_asin_a1_div_l ** 4 * t.sin_t0_plus_asin_a2_div_l ** 4 + t.l_p8 * t.sin_t0_plus_asin_a1_div_l ** 4 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p8 * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l ** 4 - t.l_p8 * t.sin_t0_plus_asin_a1_div_l_p2 - t.l_p8 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p8) * t.sin_t0_plus_asin_a1_div_l) ** 2 * t.sin_t0_sq / ( + (-t.l_p2 * t.sin_t0_plus_asin_a1_div_l_p2 + t.l_p2) * ( + -t.l_p4 * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p4) ** 2)) * ( + t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l ** 3 - t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l - t.l_p5 * t.cos_t0 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p5 * t.sin_t0 - L * sqrt( + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l ** 3 - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l_p2 - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l ** 3 + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a2_div_l_p2 - 2 * t.l_p8 * t.sin_t0_sq - t.l_p8 * t.sin_t0_plus_asin_a1_div_l ** 4 * t.sin_t0_plus_asin_a2_div_l ** 4 + t.l_p8 * t.sin_t0_plus_asin_a1_div_l ** 4 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p8 * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l ** 4 - t.l_p8 * t.sin_t0_plus_asin_a1_div_l_p2 - t.l_p8 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p8) * t.sin_t0_plus_asin_a2_div_l) / ( + L * sqrt(-t.l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p2) * ( + -t.l_p4 * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p4) * t.sin_t0), + sqrt(4 * t.l_p2 * t.sin_t0_sq - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l ** 3 - t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l - t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p5 * t.cos_t0 - L * sqrt( + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l ** 3 - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l_p2 - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l ** 3 + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a2_div_l_p2 - 2 * t.l_p8 * t.sin_t0_sq - t.l_p8 * t.sin_t0_plus_asin_a1_div_l ** 4 * t.sin_t0_plus_asin_a2_div_l ** 4 + t.l_p8 * t.sin_t0_plus_asin_a1_div_l ** 4 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p8 * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l ** 4 - t.l_p8 * t.sin_t0_plus_asin_a1_div_l_p2 - t.l_p8 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p8) * t.sin_t0_plus_asin_a2_div_l) ** 2 * t.sin_t0_sq / ( + (-t.l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p2) * ( + -t.l_p4 * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p4) ** 2) - 4 * t.l_p2 * ( + t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l - t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l_p2 - t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l + t.l_p5 * t.sin_t0 - L * sqrt( + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l ** 3 - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l_p2 - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l ** 3 + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a2_div_l_p2 - 2 * t.l_p8 * t.sin_t0_sq - t.l_p8 * t.sin_t0_plus_asin_a1_div_l ** 4 * t.sin_t0_plus_asin_a2_div_l ** 4 + t.l_p8 * t.sin_t0_plus_asin_a1_div_l ** 4 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p8 * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l ** 4 - t.l_p8 * t.sin_t0_plus_asin_a1_div_l_p2 - t.l_p8 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p8) * t.sin_t0_plus_asin_a1_div_l) ** 2 * t.sin_t0_sq / ( + (-t.l_p2 * t.sin_t0_plus_asin_a1_div_l_p2 + t.l_p2) * ( + -t.l_p4 * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p4) ** 2)) * ( + t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l - t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l_p2 - t.l_p5 * t.sin_t0 * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l + t.l_p5 * t.cos_t0 - L * sqrt( + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l ** 3 - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l ** 3 * t.sin_t0_plus_asin_a2_div_l - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l_p2 - 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l ** 3 + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a1_div_l * t.sin_t0_plus_asin_a2_div_l + 2 * t.l_p8 * t.sin_t0_sq * t.sin_t0_plus_asin_a2_div_l_p2 - 2 * t.l_p8 * t.sin_t0_sq - t.l_p8 * t.sin_t0_plus_asin_a1_div_l ** 4 * t.sin_t0_plus_asin_a2_div_l ** 4 + t.l_p8 * t.sin_t0_plus_asin_a1_div_l ** 4 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p8 * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l ** 4 - t.l_p8 * t.sin_t0_plus_asin_a1_div_l_p2 - t.l_p8 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p8) * t.sin_t0_plus_asin_a1_div_l) / ( + L * sqrt(-t.l_p2 * t.sin_t0_plus_asin_a1_div_l_p2 + t.l_p2) * ( + -t.l_p4 * t.sin_t0_plus_asin_a1_div_l_p2 * t.sin_t0_plus_asin_a2_div_l_p2 + t.l_p4) * t.sin_t0)) + + +# INVERSE KINEMATICS + + +def ik(ex, ey, L, T_0): + """ + From: + https://colab.research.google.com/drive/1a_asI3TbcC0TyEOPawE9-bi-Tq2IHzxT#scrollTo=txCaxKIJsXTz&line=1&uniqifier=1 + + PE_x*(-PE_x/2 + x)/2 + PE_y*(-PE_y/2 + y)/2 + (z - sqrt(4*L**2*sin(T_0)**2 - PE_x**2 - PE_y**2)/2)*sqrt(4*L**2*sin(T_0)**2 - PE_x**2 - PE_y**2)/2 + xline (x, -2*L**2*sin(T_0)**2 + PE_y*y + z*sqrt(4*L**2*sin(T_0)**2 - PE_x**2 - PE_y**2)) + + P1_y_s + (2*L**2*sin(T_0)**2 - (2*L**2*sqrt(4*L**2*sin(T_0)**2 - PE_x**2 - PE_y**2)*sin(T_0)**2/(4*L**2*sin(T_0)**2 - PE_x**2) - L*PE_y*sqrt(-4*L**2*sin(T_0)**4 + 4*L**2*sin(T_0)**2 - PE_x**2)/(4*L**2*sin(T_0)**2 - PE_x**2))*sqrt(4*L**2*sin(T_0)**2 - PE_x**2 - PE_y**2))/PE_y + + P3_y_s + (2*L**2*sin(T_0)**2 - (2*L**2*sqrt(4*L**2*sin(T_0)**2 - PE_x**2 - PE_y**2)*sin(T_0)**2/(4*L**2*sin(T_0)**2 - PE_x**2) + L*PE_y*sqrt(-4*L**2*sin(T_0)**4 + 4*L**2*sin(T_0)**2 - PE_x**2)/(4*L**2*sin(T_0)**2 - PE_x**2))*sqrt(4*L**2*sin(T_0)**2 - PE_x**2 - PE_y**2))/PE_y + + a1_s + 2*L**2*sqrt(4*L**2*sin(T_0)**2 - PE_x**2 - PE_y**2)*sin(T_0)**2/(4*L**2*sin(T_0)**2 - PE_x**2) - L*PE_y*sqrt(-4*L**2*sin(T_0)**4 + 4*L**2*sin(T_0)**2 - PE_x**2)/(4*L**2*sin(T_0)**2 - PE_x**2) + + P3_z_s + 2*L**2*sqrt(4*L**2*sin(T_0)**2 - PE_x**2 - PE_y**2)*sin(T_0)**2/(4*L**2*sin(T_0)**2 - PE_x**2) + L*PE_y*sqrt(-4*L**2*sin(T_0)**4 + 4*L**2*sin(T_0)**2 - PE_x**2)/(4*L**2*sin(T_0)**2 - PE_x**2) + + P2_x_s + (2*L**2*sin(T_0)**2 - sqrt(4*L**2*sin(T_0)**2 - 2*PE_x**2)*(2*L**2*sqrt(4*L**2*sin(T_0)**2 - 2*PE_x**2)*sin(T_0)**2/(4*L**2*sin(T_0)**2 - PE_x**2) - L*PE_x*sqrt(-4*L**2*sin(T_0)**4 + 4*L**2*sin(T_0)**2 - PE_x**2)/(4*L**2*sin(T_0)**2 - PE_x**2)))/PE_x + + a2_s + 2*L**2*sqrt(4*L**2*sin(T_0)**2 - 2*PE_x**2)*sin(T_0)**2/(4*L**2*sin(T_0)**2 - PE_x**2) - L*PE_x*sqrt(-4*L**2*sin(T_0)**4 + 4*L**2*sin(T_0)**2 - PE_x**2)/(4*L**2*sin(T_0)**2 - PE_x**2) + + P4_x_s + (2*L**2*sin(T_0)**2 - sqrt(4*L**2*sin(T_0)**2 - 2*PE_x**2)*(2*L**2*sqrt(4*L**2*sin(T_0)**2 - 2*PE_x**2)*sin(T_0)**2/(4*L**2*sin(T_0)**2 - PE_x**2) + L*PE_x*sqrt(-4*L**2*sin(T_0)**4 + 4*L**2*sin(T_0)**2 - PE_x**2)/(4*L**2*sin(T_0)**2 - PE_x**2)))/PE_x + + P4_z_s + 2*L**2*sqrt(4*L**2*sin(T_0)**2 - 2*PE_x**2)*sin(T_0)**2/(4*L**2*sin(T_0)**2 - PE_x**2) + L*PE_x*sqrt(-4*L**2*sin(T_0)**4 + 4*L**2*sin(T_0)**2 - PE_x**2)/(4*L**2*sin(T_0)**2 - PE_x**2) + 2*t.l_p2*sqrt(4*t.l_p2*t.sin_t0_sq - t.ex_p2 - t.ey_p2)*t.sin_t0_sq/(4*t.l_p2*t.sin_t0_sq - t.ex_p2) - L*ey*sqrt(-4*t.l_p2*t.sin_t0**4 + 4*t.l_p2*t.sin_t0_sq - t.ex_p2)/(4*t.l_p2*t.sin_t0_sq - t.ex_p2) + """ + + t = CommonTerms(L=L, T_0=T_0, ex=ex, ey=ey) + + # a1_s + a1 = 2 * t.l_p2 * sqrt(4 * t.l_p2 * t.sin_t0_sq - t.ex_p2 - t.ey_p2) * t.sin_t0_sq / ( + 4 * t.l_p2 * t.sin_t0_sq - t.ex_p2) - L * ey * sqrt( + -4 * t.l_p2 * t.sin_t0 ** 4 + 4 * t.l_p2 * t.sin_t0_sq - t.ex_p2) / (4 * t.l_p2 * t.sin_t0_sq - t.ex_p2) + + # a2_s + a2 = 2 * t.l_p2 * sqrt(4 * t.l_p2 * t.sin_t0_sq - 2 * t.ex_p2) * t.sin_t0_sq / ( + 4 * t.l_p2 * t.sin_t0_sq - t.ex_p2) - L * ex * sqrt( + -4 * t.l_p2 * t.sin_t0 ** 4 + 4 * t.l_p2 * t.sin_t0_sq - t.ex_p2) / (4 * t.l_p2 * t.sin_t0_sq - t.ex_p2) + + return np.array([a1, a2]) diff --git a/python/hemisphere-joint-demo/hemisphere_joint_demo/sympy_to_code.py b/python/hemisphere-joint-demo/hemisphere_joint_demo/sympy_to_code.py new file mode 100644 index 0000000000000000000000000000000000000000..ec30271bb4240a80c30fe6fb03abedeecb773fa9 --- /dev/null +++ b/python/hemisphere-joint-demo/hemisphere_joint_demo/sympy_to_code.py @@ -0,0 +1,286 @@ +import dataclasses as dc + +import sympy as sp +import typing as ty + +from collections import OrderedDict + + +@dc.dataclass +class Line: + lhs: str + rhs: str + + def make_decl(self) -> str: + return f"double {self.lhs} = 0;" + + def make_impl(self): + return f"{self.lhs} = {self.rhs};" + + @classmethod + def lhs_from_expr(cls, expr: sp.Basic) -> str: + lhs = "_" + str(expr) + long = False + if long: + lhs = (lhs.replace(" ", "") + .replace("**", "_pow_") + .replace("+", "_add_") + .replace("-", "_sub_") + .replace("*", "_mul_") + .replace("/", "_div_") + .replace(".", "_dot_") + .replace("(", "__lpar_") + .replace(")", "_rpar__") + ) + else: + lhs = (lhs.replace(" ", "") + .replace("**", "_p_") + .replace("+", "_a_") + .replace("-", "_s_") + .replace("*", "_m_") + .replace("/", "_d_") + .replace(".", "_t_") + .replace("(", "_l_") + .replace(")", "_r_") + ) + return lhs + + @classmethod + def rhs_from_expr(cls, expr: sp.Basic, cpp: "SympyToCpp") -> str: + # Recurse. + code_args = [expr_to_cpp(arg, cpp) for arg in expr.args] + + def par(code: str) -> str: + return f"({code})" + + def op(op: str) -> str: + return par(f" {op} ".join(code_args)) + + def fn(func: str) -> str: + return f"{func}({', '.join(code_args)})" + + if isinstance(expr, sp.Add): + return op("+") + + elif isinstance(expr, sp.Subs): + return op("-") + + elif isinstance(expr, sp.Mul): + # Special case: a/b = a * (b^-1) + return op("*") + + elif isinstance(expr, sp.Pow): + assert len(code_args) == 2 + base, exponent = code_args + if exponent == "-1": + return par(f"1 / {base}") + elif exponent == "2": + return par(f"{base} * {base}") + else: + return fn("std::pow") + + elif isinstance(expr, sp.sin): + return fn("std::sin") + + elif isinstance(expr, sp.cos): + return fn("std::cos") + + elif isinstance(expr, sp.asin): + return fn("std::asin") + + elif isinstance(expr, sp.acos): + return fn("std::acos") + + elif isinstance(expr, sp.exp): + return fn("std::exp") + + else: + raise TypeError(f"{expr.__class__}, {expr.func}({expr.args}) = {sp.srepr(expr)}") + + + @classmethod + def from_expr(cls, expr: sp.Basic, cpp: "SympyToCpp"): + lhs = Line.lhs_from_expr(expr) + rhs = Line.rhs_from_expr(expr, cpp=cpp) + return cls(lhs=lhs, rhs=rhs) + + +@dc.dataclass +class SympyToCpp: + + name: str = "Expressions" + namespace: str = "VirtualRobot::hemisphere" + + function_args: ty.List[sp.Symbol] = dc.field(default_factory=list) + named_expressions: "ty.OrderedDict[sp.Basic, Line]" = dc.field(default_factory=OrderedDict) + function_results: ty.Dict[str, sp.Basic] = dc.field(default_factory=list) + + depth = 0 + indent = " " * 4 + + def build(self): + for name, expr in self.function_results.items(): + expr_to_cpp(expr, self) + + def make_compute_args(self): + return ", ".join(f"double {arg}" for arg in self.function_args) + + def make_compute_signature_decl(self): + return f"void compute({self.make_compute_args()});" + + def make_compute_signature_impl(self): + return f"void {self.name}::compute({self.make_compute_args()})" + + def make_namespace_begin_end(self): + begin = [ + f"namespace {self.namespace}", + "{", + "", + ] + end = [ + "", + "}" + ] + return begin, end + + def make_generation_note(self): + import datetime + now = datetime.datetime.now() + now = now.strftime("%Y-%m-%d %H:%M") + return [ + f"/*", + f" * This file was generated automatically on {now}.", + f" */" + "", + "", + ] + + def make_decl_lines(self) -> ty.List[str]: + lines = self._line_sum( + [ + f"class {self.name}", + "{", + "public:", + self.indent + "", + self.indent + self.make_compute_signature_decl(), + self.indent + "", + self.indent + "// Input arguments:" + ], + [self.indent + f"double {arg} = 0;" for arg in self.function_args], + [ + self.indent + "", + self.indent + "// Results:" + ], + [self.indent + f"double {res} = 0;" for res in self.function_results], + [ + self.indent + "", + self.indent + "// Intermediate expressions:" + ], + [self.indent + line.make_decl() for expr, line in self.named_expressions.items()], + [ + self.indent + "", + "};", + ] + ) + return lines + + def make_impl_lines(self) -> ty.List[str]: + lines = self._line_sum( + [ + self.make_compute_signature_impl(), + "{" + ], + [self.indent + f"this->{arg} = {arg};" for arg in self.function_args], + [ + self.indent + "", + ], + [self.indent + line.make_impl() for expr, line in self.named_expressions.items()], + [ + self.indent + "", + ], + [self.indent + Line(lhs=res, rhs=Line.lhs_from_expr(expr)).make_impl() + for res, expr in self.function_results.items()], + [ + "}", + ], + ) + return lines + + def make_header_lines(self): + ns_begin, ns_end = self.make_namespace_begin_end() + lines = self._line_sum( + self.make_generation_note(), + ["#pragma once"], + [""] * 2, + ns_begin, + self.make_decl_lines(), + ns_end, + [""] * 1, + ) + return lines + + def make_source_lines(self): + ns_begin, ns_end = self.make_namespace_begin_end() + lines = self._line_sum( + self.make_generation_note(), + [f'#include "{self.name}.h"'], + [""] * 1, + ["#include <cmath>"], + [""] * 2, + ns_begin, + self.make_impl_lines(), + ns_end, + [""] * 1, + ) + return lines + + @classmethod + def format_lines(cls, lines: ty.List[str], line_numbers=False) -> str: + if line_numbers: + lines = [f"{i:>3} | {line}" for i, line in enumerate(lines)] + return "\n".join(lines) + + @classmethod + def write_lines(cls, lines: ty.List[str], filepath: str): + with open(filepath, "w") as file: + file.writelines([l.rstrip() + "\n" for l in lines]) + + def _line_sum(self, *args): + return sum(args, []) + + +def expr_to_cpp( + expr: sp.Basic, + cpp: SympyToCpp, +) -> str: + indent = " " * cpp.depth + + if len(expr.args) == 0: + # Leaf. + print(f"{indent}Leaf: {expr}") + + if isinstance(expr, sp.Symbol): + # Must be part of local variables. + assert expr in cpp.function_args + return str(expr) + + elif isinstance(expr, sp.Number): + # Will be turned into a literal. + # The number can also be something like (1/2), where "1 / 2" would be 0 in C++. + # So we pre-evaluate these constants in Python, and pass the result literal to C++. + return str(eval(str(expr))) + + else: + raise TypeError(f"Got expr {expr} of type {type(expr)}") + + else: + # Non-leaf + print(f"{indent}Node: {expr}") + cpp.depth += 1 + + line = Line.from_expr(expr, cpp=cpp) + cpp.named_expressions[expr] = line + + cpp.depth -= 1 + + return line.lhs 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 0000000000000000000000000000000000000000..06ba866fbdb5ef17e7cc86d8e40dd29874494cb1 --- /dev/null +++ b/python/hemisphere-joint-demo/hemisphere_joint_demo/terms.py @@ -0,0 +1,96 @@ +import numpy as np + +from numpy import pi, sin, cos, sqrt, arcsin, arctan2 + + +class CommonTerms: + + def __init__( + self, + L, + T_0, + a1=None, + a2=None, + ex=None, + ey=None, + ez=None, + ): + t = self + + 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 + + self.l_p2 = L ** 2 + self.l_p3 = L ** 3 + self.l_p4 = L ** 4 + self.l_p5 = L ** 5 + self.l_p6 = L ** 6 + self.l_p8 = L ** 8 + + self.two_l_p2_mul_sin_t0_sq = (2 * self.l_p2 * self.sin_t0_sq) + if None not in [ex, ey]: + self.ex_p2 = ex ** 2 + self.ey_p2 = ey ** 2 + self.ex_mul_ey = ex * ey + self.sqrt_4_l_p2_mul_sin_t0_sq_mul_ex_sq_mul_ey_sq = sqrt(4 * self.l_p2 * self.sin_t0_sq - self.ex_p2 - self.ey_p2) + self.sqrt_4_l_p2_mul_sin_t0_sq_mul_ex_sq_mul_ey_sq_div_two_l_p2_mul_sin_t0_sq = ( + self.sqrt_4_l_p2_mul_sin_t0_sq_mul_ex_sq_mul_ey_sq / self.two_l_p2_mul_sin_t0_sq) + + if a1 is not None: + self.a1_p2 = a1 ** 2 + self.a1_p3 = a1 ** 3 + self.a1_p4 = a1 ** 4 + + self.asin_a1_div_l = arcsin(a1 / L) + self.sin_t0_plus_asin_a1_div_l = sin(T_0 + arcsin(a1 / L)) + self.sin_t0_plus_asin_a1_div_l_p2 = self.sin_t0_plus_asin_a1_div_l ** 2 + if a2 is not None: + self.a2_p2 = a2 ** 2 + self.a2_p3 = a2 ** 3 + self.a2_p4 = a2 ** 4 + + self.asin_a2_div_l = arcsin(a2 / L) + self.sin_t0_plus_asin_a2_div_l = sin(T_0 + arcsin(a2 / L)) + self.sin_t0_plus_asin_a2_div_l_p2 = self.sin_t0_plus_asin_a2_div_l ** 2 + + if a1 is not None and a2 is not None: + self.l_p4_minus_a1_sq_mul_a2_sq = (self.l_p4 - a1 ** 2 * a2 ** 2) + + self.sqrt_01 = sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 + - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq + - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4 + ) + self.sqrt_02 = sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 + - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq + - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4 + ) + self.sqrt_03 = sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 + - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq + - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4 + ) + self.sqrt_04 = sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 + - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq + - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4 + ) + self.sqrt_05 = sqrt( + -2 * t.l_p8 * t.sin_t0_sq + t.l_p8 + 2 * t.l_p6 * t.a1_p2 * t.sin_t0_sq - t.l_p6 * t.a1_p2 + + 2 * t.l_p6 * a1 * a2 * t.sin_t0_sq + 2 * t.l_p6 * t.a2_p2 * t.sin_t0_sq - t.l_p6 * t.a2_p2 + - 2 * t.l_p4 * t.a1_p3 * a2 * t.sin_t0_sq - 2 * t.l_p4 * t.a1_p2 * t.a2_p2 * t.sin_t0_sq + - 2 * t.l_p4 * a1 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p4 * t.a2_p2 + + 2 * t.l_p2 * t.a1_p3 * t.a2_p3 * t.sin_t0_sq + t.l_p2 * t.a1_p2 * t.a2_p4 - t.a1_p4 * t.a2_p4 + ) diff --git a/python/hemisphere-joint-demo/poetry.toml b/python/hemisphere-joint-demo/poetry.toml new file mode 100644 index 0000000000000000000000000000000000000000..ab1033bd37224ee84b5862fb25f094db73809b74 --- /dev/null +++ b/python/hemisphere-joint-demo/poetry.toml @@ -0,0 +1,2 @@ +[virtualenvs] +in-project = true diff --git a/python/hemisphere-joint-demo/pyproject.toml b/python/hemisphere-joint-demo/pyproject.toml new file mode 100644 index 0000000000000000000000000000000000000000..4a155e8e7faba50e85afafa8e66a9caf43e18dbc --- /dev/null +++ b/python/hemisphere-joint-demo/pyproject.toml @@ -0,0 +1,29 @@ +[tool.poetry] +name = "hemisphere-joint-demo" +version = "0.1.0" +description = "" +authors = ["Rainer Kartmann <rainer dot kartmann at kit dot edu>"] + +[tool.poetry.dependencies] +python = "^3.6" +# armarx-dev = { path = "../../../armarx/python3-armarx/", develop=true} # "^0.16.4" +zeroc-ice = "3.7.0" +numpy = "^1.19.5" +jupyter = "^1.0.0" +plotly = "*" +matplotlib = "*" +scipy = "*" +sympy = "*" + +[tool.poetry.dev-dependencies] +pytest = "^5.2" +ipython = "5.5" + +[build-system] +requires = ["poetry-core@https://github.com/python-poetry/poetry-core/archive/325312c016d69189ac93c945ba0c1b69296c5e54.zip"] +build-backend = "poetry.core.masonry.api" + +[[tool.poetry.source]] +name = "h2t" +url = "https://pypi.humanoids.kit.edu/simple/" + diff --git a/python/hemisphere-joint-demo/tests/__init__.py b/python/hemisphere-joint-demo/tests/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391